Revision 22b85da1 devices/DiWheelDrive/main.cpp
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 708 | 708 |
int proxyL = 0; |
| 709 | 709 |
int proxyR = 0; |
| 710 | 710 |
int maxDelta = 0; |
| 711 |
int sensorL = 0; |
|
| 712 |
int sensorR = 0; |
|
| 711 | 713 |
|
| 712 | 714 |
if (argc == 1){
|
| 713 | 715 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| ... | ... | |
| 739 | 741 |
proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
| 740 | 742 |
proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
| 741 | 743 |
} |
| 744 |
sensorL += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 745 |
sensorR += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 742 | 746 |
|
| 743 | 747 |
// if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
| 744 | 748 |
// delta *= -1; |
| ... | ... | |
| 754 | 758 |
// sleep(CAN::UPDATE_PERIOD); |
| 755 | 759 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 756 | 760 |
} |
| 757 |
chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: %d, FR: %d\n", maxDelta, proxyL, proxyR); |
|
| 761 |
|
|
| 758 | 762 |
|
| 759 |
global.threshProxyL = proxyL; |
|
| 760 |
global.threshProxyR = proxyR; |
|
| 763 |
global.threshProxyL = sensorL / rounds; |
|
| 764 |
global.threshProxyR = sensorR / rounds; |
|
| 765 |
chprintf(chp,"Thresh FL: %d, FR: %d\n", global.threshProxyL, global.threshProxyR); |
|
| 761 | 766 |
return; |
| 762 | 767 |
} |
| 763 | 768 |
|
| ... | ... | |
| 770 | 775 |
uint16_t proxyL = global.threshProxyL; |
| 771 | 776 |
uint16_t proxyR = global.threshProxyR; |
| 772 | 777 |
uint16_t maxDelta = 0; |
| 773 |
|
|
| 778 |
|
|
| 779 |
int sensorL = 0; |
|
| 780 |
int sensorR = 0; |
|
| 774 | 781 |
if (argc == 1){
|
| 775 | 782 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 776 | 783 |
rounds = atoi(argv[0]); |
| ... | ... | |
| 783 | 790 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 784 | 791 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 785 | 792 |
} |
| 786 |
|
|
| 793 |
|
|
| 787 | 794 |
uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
| 788 | 795 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
| 789 | 796 |
// // Update proximity thresh |
| ... | ... | |
| 810 | 817 |
return; |
| 811 | 818 |
} |
| 812 | 819 |
|
| 820 |
// Either 0 to disable record or > 0 to enable it |
|
| 821 |
void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 822 |
if (argc == 1){
|
|
| 823 |
chprintf(chp, "Set recording to %d\n", atoi(argv[0])); |
|
| 824 |
global.enableRecord = atoi(argv[0]); |
|
| 825 |
} |
|
| 826 |
} |
|
| 813 | 827 |
|
| 814 | 828 |
|
| 815 | 829 |
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| ... | ... | |
| 822 | 836 |
int maxDelta = 0; |
| 823 | 837 |
float KCrit = 0.0f; |
| 824 | 838 |
global.sensSamples = 0; |
| 839 |
global.maxDist.error = 0; |
|
| 825 | 840 |
LineFollow lf(&global); |
| 841 |
int led = 0; |
|
| 826 | 842 |
|
| 827 | 843 |
if (argc == 2){
|
| 828 | 844 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
| 829 | 845 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
| 830 | 846 |
KCrit = atof(argv[0]); |
| 831 | 847 |
steps = atoi(argv[1]); |
| 832 |
} else{
|
|
| 833 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>"); |
|
| 848 |
} else if (argc == 3){
|
|
| 849 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
|
| 850 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
|
| 851 |
KCrit = atof(argv[0]); |
|
| 852 |
steps = atoi(argv[1]); |
|
| 853 |
global.forwardSpeed = atoi(argv[2]); |
|
| 854 |
|
|
| 855 |
}else{
|
|
| 856 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps> (<speed>)"); |
|
| 834 | 857 |
return; |
| 835 | 858 |
} |
| 859 |
global.K_p = KCrit; |
|
| 860 |
for(led=0; led<8; led++){
|
|
| 861 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 862 |
} |
|
| 836 | 863 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
| 837 | 864 |
BaseThread::sleep(MS2ST(5000)); |
| 838 | 865 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 839 |
|
|
| 866 |
int checkWhite = 0; |
|
| 867 |
int it_switch = steps / 2; |
|
| 840 | 868 |
for(int s=0; s < steps; s++){
|
| 841 |
chprintf(chp,"S:%d,",s); |
|
| 842 |
lf.calibrateZiegler(KCrit, rpmSpeed); |
|
| 869 |
// chprintf(chp,"S:%d,",s); |
|
| 870 |
if(global.threshWhite) |
|
| 871 |
if(s < it_switch){
|
|
| 843 | 872 |
|
| 844 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 873 |
checkWhite = lf.followRightEdge(rpmSpeed); |
|
| 874 |
}else{
|
|
| 875 |
checkWhite = lf.followLeftEdge(rpmSpeed); |
|
| 876 |
} |
|
| 877 |
if(checkWhite){
|
|
| 878 |
for(led=0; led<8; led++){
|
|
| 879 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 880 |
} |
|
| 881 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 882 |
}else{
|
|
| 883 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 884 |
} |
|
| 885 |
|
|
| 886 |
|
|
| 845 | 887 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 846 | 888 |
} |
| 889 |
|
|
| 847 | 890 |
global.motorcontrol.setTargetRPM(0,0); |
| 848 | 891 |
} |
| 849 | 892 |
|
| ... | ... | |
| 884 | 927 |
|
| 885 | 928 |
global.senseRec[j].FL = FL; |
| 886 | 929 |
global.senseRec[j].FR = FR; |
| 887 |
global.senseRec[j].delta = delta; |
|
| 888 | 930 |
// chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
| 889 | 931 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| 890 | 932 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
| ... | ... | |
| 905 | 947 |
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 906 | 948 |
|
| 907 | 949 |
for (int j=0; j<global.sensSamples;j++){
|
| 908 |
chprintf(chp,"FL:%d,FR:%d,Delta:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta);
|
|
| 950 |
chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);
|
|
| 909 | 951 |
} |
| 952 |
chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error); |
|
| 953 |
|
|
| 910 | 954 |
|
| 911 | 955 |
} |
| 912 | 956 |
|
| ... | ... | |
| 943 | 987 |
{"calibrate_line", calibrateLineSensores},
|
| 944 | 988 |
{"record_move_l", recordMove},
|
| 945 | 989 |
{"print_record", printMove},
|
| 990 |
{"setRecord", setRecord},
|
|
| 946 | 991 |
{NULL, NULL}
|
| 947 | 992 |
}; |
| 948 | 993 |
|
Also available in: Unified diff