Revision 98e7c69b devices/DiWheelDrive/main.cpp
devices/DiWheelDrive/main.cpp | ||
---|---|---|
701 | 701 |
* Note: invert the threshs to drive on the other edge. |
702 | 702 |
* |
703 | 703 |
* */ |
704 |
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
704 |
void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
705 | 705 |
// int vcnl4020AmbientLight[4]; |
706 | 706 |
int vcnl4020Proximity[4]; |
707 | 707 |
int rounds = 1; |
... | ... | |
768 | 768 |
|
769 | 769 |
|
770 | 770 |
|
771 |
void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
771 |
void sellRequestgetBottomSensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
772 | 772 |
// uint16_t vcnl4020AmbientLight[4]; |
773 | 773 |
uint16_t vcnl4020Proximity[4]; |
774 | 774 |
uint16_t rounds = 1; |
... | ... | |
780 | 780 |
} else { |
781 | 781 |
chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n"); |
782 | 782 |
} |
783 |
global.motorcontrol.getGains(&global.motorConfigGains); |
|
784 |
global.motorcontrol.setGains(&global.stopGains); |
|
783 |
global.motorcontrol.setMotorEnable(false); |
|
785 | 784 |
|
786 | 785 |
for (int j = 0; j < rounds; j++) { |
787 | 786 |
for (int i = 0; i < 4; i++) { |
... | ... | |
801 | 800 |
// sleep(CAN::UPDATE_PERIOD); |
802 | 801 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
803 | 802 |
} |
804 |
global.motorcontrol.setGains(&global.motorConfigGains);
|
|
803 |
global.motorcontrol.setMotorEnable(true);
|
|
805 | 804 |
// chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR); |
806 | 805 |
return; |
807 | 806 |
} |
808 | 807 |
|
809 |
// Either 0 to disable record or > 0 to enable it |
|
810 |
void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
811 |
if (argc == 1){ |
|
812 |
chprintf(chp, "Set recording to %d\n", atoi(argv[0])); |
|
813 |
global.enableRecord = atoi(argv[0]); |
|
814 |
} |
|
815 |
} |
|
816 |
|
|
817 |
|
|
818 |
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) { |
|
819 |
// int vcnl4020AmbientLight[4]; |
|
820 |
// int vcnl4020Proximity[4]; |
|
821 |
int rpmSpeed[2] = {0}; |
|
822 |
int steps = 0; |
|
823 |
// int proxyL = global.threshProxyL; |
|
824 |
// int proxyR = global.threshProxyR; |
|
825 |
int maxDelta = 0; |
|
826 |
float KCrit = 0.0f; |
|
827 |
|
|
828 |
global.sensSamples = 0; |
|
829 |
global.maxDist.error = 0; |
|
830 |
LineFollow lf(&global); |
|
831 |
int led = 0; |
|
832 |
|
|
833 |
if (argc == 2){ |
|
834 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
|
835 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
|
836 |
KCrit = atof(argv[0]); |
|
837 |
steps = atoi(argv[1]); |
|
838 |
} else if (argc == 3){ |
|
839 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
|
840 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
|
841 |
global.K_p = atof(argv[0]); |
|
842 |
global.K_d = atof(argv[1]); |
|
843 |
steps = atoi(argv[2]); |
|
844 |
global.forwardSpeed = 15; |
|
845 |
|
|
846 |
}else{ |
|
847 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <kd> <steps> (<speed>)"); |
|
848 |
return; |
|
849 |
} |
|
850 |
for(led=0; led<8; led++){ |
|
851 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
852 |
} |
|
853 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
854 |
BaseThread::sleep(MS2ST(5000)); |
|
855 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
856 |
int checkWhite = 0; |
|
857 |
int it_switch = steps / 2; |
|
858 |
// lf.setStrategie(LineFollowStrategie::MIDDLE); |
|
859 |
for(int s=0; s < steps; s++){ |
|
860 |
|
|
861 |
checkWhite = lf.followLine(rpmSpeed); |
|
862 |
checkWhite = 0; |
|
863 |
// chprintf(chp,"S:%d,",s); |
|
864 |
// if(global.threshWhite) |
|
865 |
// if(s < it_switch){ |
|
866 |
// lf.setStrategie(LineFollowStrategie::EDGE_LEFT); |
|
867 |
// checkWhite = lf.followLine(rpmSpeed); |
|
868 |
// }else{ |
|
869 |
// lf.setStrategie(LineFollowStrategie::EDGE_RIGHT); |
|
870 |
// checkWhite = lf.followLine(rpmSpeed); |
|
871 |
// } |
|
872 |
if(checkWhite){ |
|
873 |
global.motorcontrol.setTargetRPM(0,0); |
|
874 |
for(led=0; led<8; led++){ |
|
875 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
876 |
} |
|
877 |
}else{ |
|
878 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
879 |
} |
|
880 |
|
|
881 |
|
|
882 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
883 |
} |
|
884 |
|
|
885 |
global.motorcontrol.setTargetRPM(0,0); |
|
886 |
} |
|
887 |
|
|
888 |
|
|
889 |
void followLine(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
890 |
int steps = 1000; |
|
891 |
int speed = 0; |
|
892 |
int strategy = 0; |
|
893 |
int led = 0; |
|
894 |
int checkWhite = 0; |
|
895 |
int rpmSpeed[2] = {0}; |
|
896 |
LineFollow lf(&global); |
|
897 |
if (argc == 1){ |
|
898 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
899 |
steps = atoi(argv[0]); |
|
900 |
}else if (argc == 2){ |
|
901 |
steps = atoi(argv[0]); |
|
902 |
speed = atoi(argv[1]); |
|
903 |
}else if (argc == 3){ |
|
904 |
steps = atoi(argv[0]); |
|
905 |
speed = atoi(argv[1]); |
|
906 |
strategy = atoi(argv[2]); |
|
907 |
}else{ |
|
908 |
chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n"); |
|
909 |
return; |
|
910 |
} |
|
911 |
global.forwardSpeed = speed; |
|
912 |
switch (strategy) |
|
913 |
{ |
|
914 |
case 0: |
|
915 |
lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT); |
|
916 |
break; |
|
917 |
case 1: |
|
918 |
lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT); |
|
919 |
break; |
|
920 |
case 2: |
|
921 |
lf.setStrategy(amiro::LineFollowStrategy::FUZZY); |
|
922 |
break; |
|
923 |
default: |
|
924 |
break; |
|
925 |
} |
|
926 |
|
|
927 |
|
|
928 |
for(int s=0; s < steps; s++){ |
|
929 |
|
|
930 |
checkWhite = lf.followLine(rpmSpeed); |
|
931 |
if(checkWhite){ |
|
932 |
global.motorcontrol.setTargetRPM(0,0); |
|
933 |
for(led=0; led<8; led++){ |
|
934 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
935 |
} |
|
936 |
}else{ |
|
937 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
938 |
} |
|
939 |
|
|
940 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
941 |
} |
|
942 |
|
|
943 |
global.motorcontrol.setTargetRPM(0,0); |
|
944 |
} |
|
945 |
|
|
946 |
|
|
947 |
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
948 |
|
|
949 |
for (int j=0; j<global.sensSamples;j++){ |
|
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); |
|
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 |
|
|
954 |
|
|
955 |
} |
|
956 |
void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
957 |
if (global.resetProtect){ |
|
958 |
global.motorcontrol.getGains(&global.motorConfigGains); |
|
959 |
global.resetProtect = 0; |
|
960 |
} |
|
961 |
|
|
962 |
global.motorcontrol.setGains(&global.stopGains); |
|
963 |
} |
|
964 |
|
|
965 |
|
|
966 |
void setGains(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
967 |
if(!global.resetProtect){ |
|
968 |
global.motorcontrol.setGains(&global.motorConfigGains); |
|
969 |
global.resetProtect = 1; |
|
970 |
} |
|
971 |
} |
|
972 |
void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
973 |
global.odometry.resetPosition(); |
|
974 |
global.motorcontrol.toggleMotorEnable(); |
|
975 |
} |
|
976 |
|
|
977 |
|
|
978 |
void setSpeed(int (&rpmSpeed)[2]){ |
|
979 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
980 |
} |
|
981 |
|
|
982 |
|
|
983 |
void enableCharging(){ |
|
984 |
global.ltc4412.enable(true); |
|
985 |
} |
|
986 |
|
|
987 |
void disableCharging(){ |
|
988 |
global.ltc4412.enable(false); |
|
989 |
} |
|
990 |
|
|
991 |
|
|
992 |
void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
993 |
uint8_t strategy = 0; |
|
994 |
if(argc == 1){ |
|
995 |
strategy = atoi(argv[0]); |
|
996 |
} |
|
997 |
// send over can |
|
998 |
global.strategyTest = strategy; |
|
999 |
global.triggerCan = true; |
|
1000 |
} |
|
1001 |
|
|
1002 |
|
|
1003 |
|
|
1004 |
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
1005 |
types::position pos = global.odometry.getPosition(); |
|
1006 |
chprintf(chp, "Start: Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z); |
|
1007 |
chprintf(chp, "End: Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d\n", global.endPos.x, global.endPos.y, global.endPos.f_x, global.endPos.f_y, global.endPos.f_z); |
|
1008 |
|
|
1009 |
if(argc == 1){ |
|
1010 |
for (int i=0; i<atoi(argv[0]);i++){ |
|
1011 |
types::position pos = global.odometry.getPosition(); |
|
1012 |
chprintf(chp, "End: Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z); |
|
1013 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1014 |
} |
|
1015 |
} |
|
1016 |
} |
|
1017 |
|
|
1018 |
// TODO: Not wokring, either loading station has no power or logic not working |
|
1019 |
void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
808 |
void shellRequestCheckPower(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
1020 | 809 |
int steps = 2000; |
1021 | 810 |
int led = 0; |
1022 |
// enableCharging(); |
|
1023 |
// for (int i=0; i<steps;i++){ |
|
1024 |
// chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); |
|
1025 |
// if(global.ltc4412.isPluggedIn()){ |
|
1026 |
// // enableCharging(); |
|
1027 |
// for(led=0; led<8; led++){ |
|
1028 |
// global.robot.setLightColor(led, Color(Color::GREEN)); |
|
1029 |
// } |
|
1030 |
// }else{ |
|
1031 |
// // disableCharging(); |
|
1032 |
// for(led=0; led<8; led++){ |
|
1033 |
// global.robot.setLightColor(led, Color(Color::RED)); |
|
1034 |
// } |
|
1035 |
// } |
|
1036 |
// BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1037 |
// } |
|
1038 |
// disableCharging(); |
|
1039 |
// for(led=0; led<8; led++){ |
|
1040 |
// global.robot.setLightColor(led, Color(Color::BLACK)); |
|
1041 |
// } |
|
1042 |
// chprintf((BaseSequentialStream*) &SD1, "Charging Flags: %d, State of charge: %d, Minutes: %d, Consumption: %d\n", global.powerFrame.data8[0], global.powerFrame.data8[1], global.powerFrame.data16[1], global.powerFrame.data16[2]); |
|
811 |
|
|
1043 | 812 |
chprintf(chp, "Power over Pins: %s Pins Enabled: %s, Power: %d\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y" : "n", global.robot.getPowerStatus().state_of_charge); |
1044 | 813 |
} |
1045 | 814 |
|
1046 | 815 |
|
1047 |
void proxyRing(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
816 |
void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
1048 | 817 |
int steps = 10000; |
1049 | 818 |
int i; |
1050 | 819 |
|
... | ... | |
1098 | 867 |
{"motor_stop", shellRequestMotorStop}, |
1099 | 868 |
{"motor_calibrate", shellRequestMotorCalibrate}, |
1100 | 869 |
{"motor_getGains", shellRequestMotorGetGains}, |
1101 |
{"motor_deactivate", freeGains}, |
|
1102 |
{"motor_activate", setGains}, |
|
1103 | 870 |
{"motor_resetGains", shellRequestMotorResetGains}, |
1104 |
{"motorToggle", motorToggle}, |
|
1105 |
{"dev_proxi_sensor_data", getProxySensorData}, |
|
1106 |
{"dev_ziegler2", zieglerMeth2}, |
|
1107 |
// TODO: Stop user process from execution to finish/force calibration before anything starts |
|
1108 |
{"calibrate_line", calibrateLineSensores}, |
|
1109 |
// {"record_move", recordMove}, |
|
1110 |
{"print_record", printMove}, |
|
1111 |
{"setRecord", setRecord}, |
|
1112 |
{"followLine", followLine}, |
|
1113 |
{"getPos", getPosition}, |
|
1114 |
{"checkPower", checkPower}, |
|
1115 |
{"setStrategy", setGlobalStrategy}, |
|
1116 |
{"proxyRing", proxyRing}, |
|
871 |
{"calibrate_line_sensors", shellRequestCalibrateLineSensores}, |
|
872 |
{"printProxyBottom", sellRequestgetBottomSensorData}, |
|
873 |
{"printProxyRing", shellRequestProxyRingValues}, |
|
874 |
{"checkPowerPins", shellRequestCheckPower}, |
|
1117 | 875 |
{NULL, NULL} |
1118 | 876 |
}; |
1119 | 877 |
|
Also available in: Unified diff