Revision c0757912
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1025 | 1025 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 1026 | 1026 |
} |
| 1027 | 1027 |
|
| 1028 |
/** |
|
| 1029 |
* Blocks as long as the position changes. |
|
| 1030 |
*/ |
|
| 1031 |
void checkForMotion(){
|
|
| 1032 |
int motion = 1; |
|
| 1033 |
int led = 0; |
|
| 1034 |
types::position oldPos = global.odometry.getPosition(); |
|
| 1035 |
while(motion){
|
|
| 1036 |
BaseThread::sleep(500); |
|
| 1037 |
types::position tmp = global.odometry.getPosition(); |
|
| 1038 |
motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
|
| 1039 |
oldPos = tmp; |
|
| 1040 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
|
| 1041 |
global.robot.setLightColor(led % 8, Color(Color::BLACK)); |
|
| 1042 |
led++; |
|
| 1043 |
} |
|
| 1044 |
} |
|
| 1045 |
|
|
| 1046 |
/** |
|
| 1047 |
* Turns MotorControl off and checks if position remains stable. |
|
| 1048 |
*/ |
|
| 1049 |
int checkDockingSuccess(){
|
|
| 1050 |
// global.motorcontrol.setTargetRPM(0,0); |
|
| 1051 |
checkForMotion(); |
|
| 1052 |
int led = 0; |
|
| 1053 |
int success = 0; |
|
| 1054 |
global.odometry.resetPosition(); |
|
| 1055 |
types::position start = global.startPos = global.odometry.getPosition(); |
|
| 1056 |
global.motorcontrol.toggleMotorEnable(); |
|
| 1057 |
BaseThread::sleep(1000); |
|
| 1058 |
types::position stop = global.endPos = global.odometry.getPosition(); |
|
| 1059 |
global.motorcontrol.toggleMotorEnable(); |
|
| 1060 |
// Amiro moved, docking was not successful |
|
| 1061 |
if ((start.x + stop.x) || (start.y + stop.y)){
|
|
| 1062 |
for(led=0; led<8; led++){
|
|
| 1063 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 1064 |
} |
|
| 1065 |
success = 0; |
|
| 1066 |
}else{
|
|
| 1067 |
for(led=0; led<8; led++){
|
|
| 1068 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
| 1069 |
} |
|
| 1070 |
success = 1; |
|
| 1071 |
} |
|
| 1072 |
|
|
| 1073 |
BaseThread::sleep(500); |
|
| 1074 |
for(led=0; led<8; led++){
|
|
| 1075 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 1076 |
} |
|
| 1077 |
return success; |
|
| 1078 |
} |
|
| 1079 |
|
|
| 1080 |
/** |
|
| 1081 |
* Switch to EDGE_LEFT line following and orientate for n steps along the line. |
|
| 1082 |
* |
|
| 1083 |
* @param lf current line following |
|
| 1084 |
* @param steps steps to take for the correction (measured in CAN-update durations) |
|
| 1085 |
*/ |
|
| 1086 |
void correctPosition(LineFollow &lf, int steps){
|
|
| 1087 |
int checkWhite = 0; |
|
| 1088 |
int rpmSpeed[2] = {0};
|
|
| 1089 |
lf.setStrategy(LineFollowStrategy::EDGE_LEFT); |
|
| 1090 |
for (int correction=0; correction<steps; correction++){
|
|
| 1091 |
checkWhite = lf.followLine(rpmSpeed); |
|
| 1092 |
setSpeed(rpmSpeed); |
|
| 1093 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1094 |
} |
|
| 1095 |
} |
|
| 1096 |
|
|
| 1097 |
|
|
| 1098 |
|
|
| 1028 | 1099 |
void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 1029 | 1100 |
int steps = 10000; |
| 1030 | 1101 |
int speed = 0; |
| ... | ... | |
| 1032 | 1103 |
int led = 0; |
| 1033 | 1104 |
int checkWhite = 0; |
| 1034 | 1105 |
int rpmSpeed[2] = {0};
|
| 1035 |
int proxyWheelThresh = 13000;
|
|
| 1106 |
int proxyWheelThresh = 20000;
|
|
| 1036 | 1107 |
LineFollow lf(&global); |
| 1037 | 1108 |
if (argc == 1){
|
| 1038 | 1109 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
| ... | ... | |
| 1048 | 1119 |
BaseThread::sleep(MS2ST(5000)); |
| 1049 | 1120 |
global.forwardSpeed = speed; |
| 1050 | 1121 |
|
| 1051 |
// rpmSpeed[0] = -speed; |
|
| 1052 |
// rpmSpeed[1] = speed; |
|
| 1053 | 1122 |
setSpeed(rpmSpeed); |
| 1054 |
// lf.setStrategy(LineFollowStrategy::DOCKING); |
|
| 1123 |
|
|
| 1124 |
|
|
| 1055 | 1125 |
for(int s=0; s < steps; s++){
|
| 1056 | 1126 |
|
| 1057 | 1127 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
| 1058 | 1128 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
| 1059 | 1129 |
if ((WL+WR) < proxyWheelThresh){
|
| 1130 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 1060 | 1131 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n"); |
| 1061 | 1132 |
if(lf.getStrategy() == LineFollowStrategy::DOCKING){
|
| 1062 |
break; |
|
| 1133 |
// Check if Docking was successful |
|
| 1134 |
if(checkDockingSuccess()){
|
|
| 1135 |
break; |
|
| 1136 |
}else{
|
|
| 1137 |
correctPosition(lf, 250); |
|
| 1138 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
| 1139 |
// break; |
|
| 1140 |
} |
|
| 1141 |
|
|
| 1063 | 1142 |
}else{
|
| 1064 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 1143 |
// global.motorcontrol.setTargetRPM(0,0);
|
|
| 1065 | 1144 |
BaseThread::sleep(1000); |
| 1145 |
// 180° Rotation |
|
| 1066 | 1146 |
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 |
} |
|
| 1147 |
// BaseThread::sleep(8000); |
|
| 1148 |
checkForMotion(); |
|
| 1149 |
correctPosition(lf, 250); |
|
| 1074 | 1150 |
// break; |
| 1075 | 1151 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
| 1076 | 1152 |
|
| ... | ... | |
| 1081 | 1157 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 1082 | 1158 |
} |
| 1083 | 1159 |
|
| 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(); |
|
| 1160 |
|
|
| 1091 | 1161 |
} |
| 1092 | 1162 |
|
| 1093 | 1163 |
|
| ... | ... | |
| 1130 | 1200 |
types::position pos = global.odometry.getPosition(); |
| 1131 | 1201 |
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 | 1202 |
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); |
| 1203 |
|
|
| 1204 |
if(argc == 1){
|
|
| 1205 |
for (int i=0; i<atoi(argv[0]);i++){
|
|
| 1206 |
types::position pos = global.odometry.getPosition(); |
|
| 1207 |
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); |
|
| 1208 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1209 |
} |
|
| 1210 |
} |
|
| 1133 | 1211 |
} |
| 1134 | 1212 |
|
| 1135 | 1213 |
static const ShellCommand commands[] = {
|
Also available in: Unified diff