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