Revision 1b3adcdd devices/DiWheelDrive/main.cpp
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 702 | 702 |
* |
| 703 | 703 |
* */ |
| 704 | 704 |
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 705 |
int vcnl4020AmbientLight[4]; |
|
| 705 |
// int vcnl4020AmbientLight[4];
|
|
| 706 | 706 |
int vcnl4020Proximity[4]; |
| 707 | 707 |
int rounds = 1; |
| 708 | 708 |
int proxyL = 0; |
| ... | ... | |
| 725 | 725 |
|
| 726 | 726 |
for (int j = 0; j < rounds; j++) {
|
| 727 | 727 |
for (int i = 0; i < 4; i++) {
|
| 728 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
| 728 |
// vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
|
|
| 729 | 729 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 730 | 730 |
} |
| 731 | 731 |
global.robot.setLightColor(j % 8, Color(Color::BLACK)); |
| ... | ... | |
| 769 | 769 |
|
| 770 | 770 |
|
| 771 | 771 |
void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 772 |
uint16_t vcnl4020AmbientLight[4]; |
|
| 772 |
// uint16_t vcnl4020AmbientLight[4];
|
|
| 773 | 773 |
uint16_t vcnl4020Proximity[4]; |
| 774 | 774 |
uint16_t rounds = 1; |
| 775 |
uint16_t proxyL = global.threshProxyL; |
|
| 776 |
uint16_t proxyR = global.threshProxyR; |
|
| 777 |
uint16_t maxDelta = 0; |
|
| 775 |
// uint16_t proxyL = global.threshProxyL;
|
|
| 776 |
// uint16_t proxyR = global.threshProxyR;
|
|
| 777 |
// uint16_t maxDelta = 0;
|
|
| 778 | 778 |
|
| 779 |
int sensorL = 0; |
|
| 780 |
int sensorR = 0; |
|
| 779 |
// int sensorL = 0;
|
|
| 780 |
// int sensorR = 0;
|
|
| 781 | 781 |
if (argc == 1){
|
| 782 | 782 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 783 | 783 |
rounds = atoi(argv[0]); |
| ... | ... | |
| 787 | 787 |
|
| 788 | 788 |
for (int j = 0; j < rounds; j++) {
|
| 789 | 789 |
for (int i = 0; i < 4; i++) {
|
| 790 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
| 790 |
// vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
|
|
| 791 | 791 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 792 | 792 |
} |
| 793 | 793 |
|
| ... | ... | |
| 827 | 827 |
|
| 828 | 828 |
|
| 829 | 829 |
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 830 |
int vcnl4020AmbientLight[4]; |
|
| 831 |
int vcnl4020Proximity[4]; |
|
| 830 |
// int vcnl4020AmbientLight[4];
|
|
| 831 |
// int vcnl4020Proximity[4];
|
|
| 832 | 832 |
int rpmSpeed[2] = {0};
|
| 833 | 833 |
int steps = 0; |
| 834 |
int proxyL = global.threshProxyL; |
|
| 835 |
int proxyR = global.threshProxyR; |
|
| 834 |
// int proxyL = global.threshProxyL;
|
|
| 835 |
// int proxyR = global.threshProxyR;
|
|
| 836 | 836 |
int maxDelta = 0; |
| 837 | 837 |
float KCrit = 0.0f; |
| 838 | 838 |
global.sensSamples = 0; |
| ... | ... | |
| 865 | 865 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 866 | 866 |
int checkWhite = 0; |
| 867 | 867 |
int it_switch = steps / 2; |
| 868 |
// lf.setStrategie(LineFollowStrategie::MIDDLE); |
|
| 868 | 869 |
for(int s=0; s < steps; s++){
|
| 870 |
|
|
| 871 |
checkWhite = lf.followLine(rpmSpeed); |
|
| 869 | 872 |
// chprintf(chp,"S:%d,",s); |
| 870 |
if(global.threshWhite) |
|
| 871 |
if(s < it_switch){
|
|
| 872 |
|
|
| 873 |
checkWhite = lf.followRightEdge(rpmSpeed); |
|
| 874 |
}else{
|
|
| 875 |
checkWhite = lf.followLeftEdge(rpmSpeed); |
|
| 876 |
} |
|
| 873 |
// if(global.threshWhite) |
|
| 874 |
// if(s < it_switch){
|
|
| 875 |
// lf.setStrategie(LineFollowStrategie::EDGE_LEFT); |
|
| 876 |
// checkWhite = lf.followLine(rpmSpeed); |
|
| 877 |
// }else{
|
|
| 878 |
// lf.setStrategie(LineFollowStrategie::EDGE_RIGHT); |
|
| 879 |
// checkWhite = lf.followLine(rpmSpeed); |
|
| 880 |
// } |
|
| 877 | 881 |
if(checkWhite){
|
| 882 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 878 | 883 |
for(led=0; led<8; led++){
|
| 879 | 884 |
global.robot.setLightColor(led, Color(Color::RED)); |
| 880 | 885 |
} |
| 881 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 882 | 886 |
}else{
|
| 883 | 887 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 884 | 888 |
} |
| ... | ... | |
| 891 | 895 |
} |
| 892 | 896 |
|
| 893 | 897 |
|
| 894 |
|
|
| 895 |
void recordMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 896 |
// int vcnl4020AmbientLight[4]; |
|
| 897 |
int vcnl4020Proximity[4]; |
|
| 898 |
int rpmSpeed[2] = {0};
|
|
| 899 |
int steps = 0; |
|
| 898 |
void followLine(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 899 |
int steps = 1000; |
|
| 900 | 900 |
int speed = 0; |
| 901 |
int strategy = 0; |
|
| 902 |
int led = 0; |
|
| 903 |
int checkWhite = 0; |
|
| 904 |
int rpmSpeed[2] = {0};
|
|
| 905 |
LineFollow lf(&global); |
|
| 901 | 906 |
if (argc == 1){
|
| 902 | 907 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
| 903 | 908 |
steps = atoi(argv[0]); |
| 904 |
speed = 30; |
|
| 905 |
}else if (argc == 2){
|
|
| 906 |
steps = atoi(argv[0]); |
|
| 907 |
speed = atoi(argv[1]); |
|
| 908 |
}else{
|
|
| 909 |
chprintf(chp, "No steps given!\n"); |
|
| 910 |
return; |
|
| 911 |
} |
|
| 912 |
global.sensSamples = steps; |
|
| 913 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
| 914 |
BaseThread::sleep(MS2ST(5000)); |
|
| 915 |
// int sensSamples = 0; |
|
| 916 |
// sensorRecord senseRec[1000]; |
|
| 917 |
|
|
| 918 |
for (int j = 0; j < steps; j++) {
|
|
| 919 |
for (int i = 0; i < 4; i++) {
|
|
| 920 |
// vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
| 921 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
|
| 909 |
}else if (argc == 2){
|
|
| 910 |
steps = atoi(argv[0]); |
|
| 911 |
speed = atoi(argv[1]); |
|
| 912 |
}else if (argc == 3){
|
|
| 913 |
steps = atoi(argv[0]); |
|
| 914 |
speed = atoi(argv[1]); |
|
| 915 |
strategy = atoi(argv[2]); |
|
| 916 |
}else{
|
|
| 917 |
chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n"); |
|
| 918 |
return; |
|
| 919 |
} |
|
| 920 |
global.forwardSpeed = speed; |
|
| 921 |
switch (strategy) |
|
| 922 |
{
|
|
| 923 |
case 0: |
|
| 924 |
lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT); |
|
| 925 |
break; |
|
| 926 |
case 1: |
|
| 927 |
lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT); |
|
| 928 |
break; |
|
| 929 |
case 2: |
|
| 930 |
lf.setStrategy(amiro::LineFollowStrategy::FUZZY); |
|
| 931 |
break; |
|
| 932 |
default: |
|
| 933 |
break; |
|
| 922 | 934 |
} |
| 923 | 935 |
|
| 924 |
int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 925 |
int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 926 |
|
|
| 927 | 936 |
|
| 928 |
global.senseRec[j].FL = FL; |
|
| 929 |
global.senseRec[j].FR = FR; |
|
| 930 |
// chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
|
| 931 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
| 932 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
| 933 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
|
| 934 |
global.motorcontrol.setTargetRPM(speed * 1000000, -speed * 1000000); |
|
| 935 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 937 |
for(int s=0; s < steps; s++){
|
|
| 938 |
|
|
| 939 |
checkWhite = lf.followLine(rpmSpeed); |
|
| 940 |
if(checkWhite){
|
|
| 941 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 942 |
for(led=0; led<8; led++){
|
|
| 943 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 944 |
} |
|
| 945 |
}else{
|
|
| 946 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 947 |
} |
|
| 948 |
|
|
| 949 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 936 | 950 |
} |
| 951 |
|
|
| 937 | 952 |
global.motorcontrol.setTargetRPM(0,0); |
| 938 |
for(int k=0; k<8;k++){
|
|
| 939 |
global.robot.setLightColor(k, Color(Color::WHITE)); |
|
| 940 |
} |
|
| 941 |
BaseThread::sleep(MS2ST(1000)); |
|
| 942 |
for(int k=0; k<8;k++){
|
|
| 943 |
global.robot.setLightColor(k, Color(Color::BLACK)); |
|
| 944 |
} |
|
| 945 | 953 |
} |
| 946 | 954 |
|
| 955 |
|
|
| 947 | 956 |
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 948 | 957 |
|
| 949 | 958 |
for (int j=0; j<global.sensSamples;j++){
|
| ... | ... | |
| 985 | 994 |
{"dev_ziegler2", zieglerMeth2},
|
| 986 | 995 |
// TODO: Stop user process from execution to finish/force calibration before anything starts |
| 987 | 996 |
{"calibrate_line", calibrateLineSensores},
|
| 988 |
{"record_move_l", recordMove},
|
|
| 997 |
// {"record_move", recordMove},
|
|
| 989 | 998 |
{"print_record", printMove},
|
| 990 | 999 |
{"setRecord", setRecord},
|
| 1000 |
{"followLine", followLine},
|
|
| 991 | 1001 |
{NULL, NULL}
|
| 992 | 1002 |
}; |
| 993 | 1003 |
|
Also available in: Unified diff