Revision bfffb0bd devices/DiWheelDrive/main.cpp
devices/DiWheelDrive/main.cpp | ||
---|---|---|
783 | 783 |
rounds = atoi(argv[0]); |
784 | 784 |
|
785 | 785 |
} |
786 |
|
|
786 |
global.motorcontrol.getGains(&global.motorConfigGains); |
|
787 |
global.motorcontrol.setGains(&global.stopGains); |
|
787 | 788 |
|
788 | 789 |
for (int j = 0; j < rounds; j++) { |
789 | 790 |
for (int i = 0; i < 4; i++) { |
... | ... | |
813 | 814 |
// sleep(CAN::UPDATE_PERIOD); |
814 | 815 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
815 | 816 |
} |
817 |
global.motorcontrol.setGains(&global.motorConfigGains); |
|
816 | 818 |
// chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR); |
817 | 819 |
return; |
818 | 820 |
} |
... | ... | |
962 | 964 |
|
963 | 965 |
|
964 | 966 |
} |
967 |
void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
968 |
if (global.resetProtect){ |
|
969 |
global.motorcontrol.getGains(&global.motorConfigGains); |
|
970 |
global.resetProtect = 0; |
|
971 |
} |
|
972 |
|
|
973 |
global.motorcontrol.setGains(&global.stopGains); |
|
974 |
} |
|
975 |
|
|
976 |
|
|
977 |
void setGains(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
978 |
if(!global.resetProtect){ |
|
979 |
global.motorcontrol.setGains(&global.motorConfigGains); |
|
980 |
global.resetProtect = 1; |
|
981 |
} |
|
982 |
} |
|
983 |
void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
984 |
global.odometry.resetPosition(); |
|
985 |
global.motorcontrol.toggleMotorEnable(); |
|
986 |
} |
|
987 |
|
|
988 |
|
|
989 |
void rotate(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
990 |
int steps = 1000; |
|
991 |
int speed = 0; |
|
992 |
int strategy = 0; |
|
993 |
int led = 0; |
|
994 |
int checkWhite = 0; |
|
995 |
int rpmSpeed[2] = {0}; |
|
996 |
LineFollow lf(&global); |
|
997 |
if (argc == 1){ |
|
998 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
999 |
speed = atoi(argv[0]); |
|
1000 |
}else if (argc == 2){ |
|
1001 |
speed = atoi(argv[0]); |
|
1002 |
steps = atoi(argv[1]); |
|
1003 |
}else{ |
|
1004 |
chprintf(chp, "Use: rotate <speed> <steps>\n"); |
|
1005 |
return; |
|
1006 |
} |
|
1007 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
1008 |
BaseThread::sleep(MS2ST(5000)); |
|
1009 |
// global.forwardSpeed = speed; |
|
1010 |
// rpmSpeed[0] = -speed; |
|
1011 |
// rpmSpeed[1] = speed; |
|
1012 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
1013 |
// for(int s=0; s < steps; s++){ |
|
1014 |
// BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1015 |
// } |
|
1016 |
global.odometry.resetPosition(); |
|
1017 |
global.startPos = global.odometry.getPosition(); |
|
1018 |
global.distcontrol.setTargetPosition(0, 3141592, 5000); |
|
1019 |
BaseThread::sleep(MS2ST(11000)); |
|
1020 |
global.endPos = global.odometry.getPosition(); |
|
1021 |
// global.motorcontrol.setTargetRPM(0,0); |
|
1022 |
} |
|
1023 |
|
|
1024 |
void setSpeed(int (&rpmSpeed)[2]){ |
|
1025 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
1026 |
} |
|
1027 |
|
|
1028 |
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
1029 |
int steps = 10000; |
|
1030 |
int speed = 0; |
|
1031 |
int strategy = 0; |
|
1032 |
int led = 0; |
|
1033 |
int checkWhite = 0; |
|
1034 |
int rpmSpeed[2] = {0}; |
|
1035 |
int proxyWheelThresh = 13000; |
|
1036 |
LineFollow lf(&global); |
|
1037 |
if (argc == 1){ |
|
1038 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
1039 |
speed = atoi(argv[0]); |
|
1040 |
}else if (argc == 2){ |
|
1041 |
speed = atoi(argv[0]); |
|
1042 |
steps = atoi(argv[1]); |
|
1043 |
}else{ |
|
1044 |
chprintf(chp, "Use: rotate <speed> <steps>\n"); |
|
1045 |
return; |
|
1046 |
} |
|
1047 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
1048 |
BaseThread::sleep(MS2ST(5000)); |
|
1049 |
global.forwardSpeed = speed; |
|
1050 |
|
|
1051 |
// rpmSpeed[0] = -speed; |
|
1052 |
// rpmSpeed[1] = speed; |
|
1053 |
setSpeed(rpmSpeed); |
|
1054 |
// lf.setStrategy(LineFollowStrategy::DOCKING); |
|
1055 |
for(int s=0; s < steps; s++){ |
|
1056 |
|
|
1057 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
1058 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
1059 |
if ((WL+WR) < proxyWheelThresh){ |
|
1060 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n"); |
|
1061 |
if(lf.getStrategy() == LineFollowStrategy::DOCKING){ |
|
1062 |
break; |
|
1063 |
}else{ |
|
1064 |
global.motorcontrol.setTargetRPM(0,0); |
|
1065 |
BaseThread::sleep(1000); |
|
1066 |
global.distcontrol.setTargetPosition(0, 3141592, 10000); |
|
1067 |
BaseThread::sleep(10000); |
|
1068 |
lf.setStrategy(LineFollowStrategy::EDGE_LEFT); |
|
1069 |
for (int correction=0; correction<300; correction++){ |
|
1070 |
checkWhite = lf.followLine(rpmSpeed); |
|
1071 |
setSpeed(rpmSpeed); |
|
1072 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1073 |
} |
|
1074 |
// break; |
|
1075 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
1076 |
|
|
1077 |
} |
|
1078 |
} |
|
1079 |
checkWhite = lf.followLine(rpmSpeed); |
|
1080 |
setSpeed(rpmSpeed); |
|
1081 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1082 |
} |
|
1083 |
|
|
1084 |
global.motorcontrol.setTargetRPM(0,0); |
|
1085 |
global.odometry.resetPosition(); |
|
1086 |
global.startPos = global.odometry.getPosition(); |
|
1087 |
global.motorcontrol.toggleMotorEnable(); |
|
1088 |
BaseThread::sleep(3000); |
|
1089 |
global.endPos = global.odometry.getPosition(); |
|
1090 |
global.motorcontrol.toggleMotorEnable(); |
|
1091 |
} |
|
1092 |
|
|
1093 |
|
|
1094 |
void getAccel(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
1095 |
int steps = 0; |
|
1096 |
// global.motorcontrol.getGains(&global.motorConfigGains); |
|
1097 |
// global.motorcontrol.setGains(&global.stopGains); |
|
1098 |
int16_t time = 10000; |
|
1099 |
int32_t angle = 3141592; |
|
1100 |
int32_t distance = 0; |
|
1101 |
if (argc == 1){ |
|
1102 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
1103 |
steps = atoi(argv[0]); |
|
1104 |
} |
|
1105 |
else if(argc == 3){ |
|
1106 |
distance = atoi(argv[0]); |
|
1107 |
angle = atoi(argv[0]); |
|
1108 |
time = atoi(argv[0]); |
|
1109 |
}else{ |
|
1110 |
chprintf(chp, "Use: accel <steps>\n"); |
|
1111 |
return; |
|
1112 |
} |
|
1113 |
global.distcontrol.setTargetPosition(distance, angle, time); |
|
1114 |
|
|
1115 |
for(int i=0; i<steps; i++){ |
|
1116 |
int16_t Z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
|
1117 |
int16_t X = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_X); |
|
1118 |
int16_t Y = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Y); |
|
1119 |
types::position pos = global.odometry.getPosition(); |
|
1120 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Axis X: %d, Y: %d, Z: %d\n", X, Y, Z); |
|
1121 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Rotation X: %d, Y: %d, Z: %d, Angle: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z, global.distcontrol.getCurrentTargetAngle()); |
|
1122 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Z: %d\n", pos.f_x, pos.f_y, pos.f_z); |
|
1123 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
1124 |
} |
|
1125 |
|
|
1126 |
// global.motorcontrol.setGains(&global.motorConfigGains); |
|
1127 |
} |
|
965 | 1128 |
|
1129 |
void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
1130 |
types::position pos = global.odometry.getPosition(); |
|
1131 |
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); |
|
1132 |
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); |
|
1133 |
} |
|
966 | 1134 |
|
967 | 1135 |
static const ShellCommand commands[] = { |
968 | 1136 |
{"shutdown", shellRequestShutdown}, |
... | ... | |
989 | 1157 |
{"motor_stop", shellRequestMotorStop}, |
990 | 1158 |
{"motor_calibrate", shellRequestMotorCalibrate}, |
991 | 1159 |
{"motor_getGains", shellRequestMotorGetGains}, |
1160 |
{"motor_deactivate", freeGains}, |
|
1161 |
{"motor_activate", setGains}, |
|
992 | 1162 |
{"motor_resetGains", shellRequestMotorResetGains}, |
1163 |
{"motorToggle", motorToggle}, |
|
993 | 1164 |
{"dev_proxi_sensor_data", proxySensorData}, |
994 | 1165 |
{"dev_ziegler2", zieglerMeth2}, |
995 | 1166 |
// TODO: Stop user process from execution to finish/force calibration before anything starts |
... | ... | |
998 | 1169 |
{"print_record", printMove}, |
999 | 1170 |
{"setRecord", setRecord}, |
1000 | 1171 |
{"followLine", followLine}, |
1172 |
{"rotate", rotate}, |
|
1173 |
{"followRotate", followAndRotate}, |
|
1174 |
{"accel", getAccel}, |
|
1175 |
{"getPos", getPosition}, |
|
1001 | 1176 |
{NULL, NULL} |
1002 | 1177 |
}; |
1003 | 1178 |
|
Also available in: Unified diff