| 80 |
80 |
global.odometry.resetPosition();
|
| 81 |
81 |
types::position start = global.startPos = global.odometry.getPosition();
|
| 82 |
82 |
global.motorcontrol.toggleMotorEnable();
|
| 83 |
|
this->sleep(1000);
|
|
83 |
this->sleep(500);
|
| 84 |
84 |
types::position stop_ = global.endPos = global.odometry.getPosition();
|
| 85 |
85 |
|
| 86 |
86 |
// Amiro moved, docking was not successful
|
| ... | ... | |
| 94 |
94 |
success = 1;
|
| 95 |
95 |
}
|
| 96 |
96 |
|
| 97 |
|
this->sleep(500);
|
|
97 |
// this->sleep(500);
|
| 98 |
98 |
lightAllLeds(Color::BLACK);
|
| 99 |
99 |
return success;
|
| 100 |
100 |
}
|
| ... | ... | |
| 129 |
129 |
|
| 130 |
130 |
int whiteBuf = 0;
|
| 131 |
131 |
int proxyBuf = 0;
|
|
132 |
int releaseBuf = 0;
|
| 132 |
133 |
int correctionStep = 0;
|
|
134 |
int dist_count = 0;
|
|
135 |
bool needDistance = false;
|
|
136 |
|
| 133 |
137 |
uint16_t rProx[8]; // buffer for ring proxy values
|
| 134 |
138 |
int rpmSpeed[2] = {0};
|
| 135 |
139 |
int stop[2] = {0};
|
|
140 |
int turn[2] = {5,-5};
|
| 136 |
141 |
LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
|
| 137 |
142 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 138 |
143 |
global.robot.setLightColor(led, Color(Color::BLACK));
|
| ... | ... | |
| 216 |
221 |
if(/* checkPinVoltage() && */ checkPinEnabled()){
|
| 217 |
222 |
global.robot.requestCharging(0);
|
| 218 |
223 |
}
|
| 219 |
|
|
|
224 |
whiteBuf = 0;
|
|
225 |
proxyBuf = 0;
|
| 220 |
226 |
break;
|
| 221 |
227 |
// ---------------------------------------
|
| 222 |
228 |
case states::FOLLOW_LINE:
|
| ... | ... | |
| 231 |
237 |
|
| 232 |
238 |
//TODO: Check if white is detected and stop threshold is reached
|
| 233 |
239 |
if(lf.followLine(rpmSpeed)){
|
| 234 |
|
|
|
240 |
whiteBuf++;
|
| 235 |
241 |
if(whiteBuf >= WHITE_COUNT_THRESH){
|
| 236 |
|
setRpmSpeed(stop);
|
|
242 |
rpmSpeed[0] = stop[0];
|
|
243 |
rpmSpeed[1] = stop[1];
|
| 237 |
244 |
newState = states::IDLE;
|
| 238 |
|
}else{
|
| 239 |
|
whiteBuf++;
|
| 240 |
245 |
}
|
| 241 |
246 |
}else{
|
| 242 |
247 |
whiteBuf = 0;
|
| 243 |
|
setRpmSpeed(rpmSpeed);
|
|
248 |
// setRpmSpeed(rpmSpeed);
|
| 244 |
249 |
}
|
| 245 |
250 |
|
| 246 |
251 |
if(getProxyRingSum() > PROXY_RING_THRESH){
|
| 247 |
|
setRpmSpeed(stop);
|
| 248 |
252 |
proxyBuf++;
|
| 249 |
253 |
if(proxyBuf > WHITE_COUNT_THRESH){
|
| 250 |
254 |
newState = states::IDLE;
|
|
255 |
rpmSpeed[0] = stop[0];
|
|
256 |
rpmSpeed[1] = stop[1];
|
| 251 |
257 |
}
|
| 252 |
258 |
}else{
|
| 253 |
259 |
proxyBuf = 0;
|
| 254 |
|
}
|
|
260 |
}
|
|
261 |
|
|
262 |
if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
|
|
263 |
rpmSpeed[0] = stop[0];
|
|
264 |
rpmSpeed[1] = stop[1];
|
|
265 |
}
|
|
266 |
|
|
267 |
// if (needDistance){
|
|
268 |
// whiteBuf = 0;
|
|
269 |
// proxyBuf = 0;
|
|
270 |
// dist_count++;
|
|
271 |
// if(dist_count > DIST_THRESH){
|
|
272 |
// dist_count = 0;
|
|
273 |
// needDistance = false;
|
|
274 |
// }
|
|
275 |
// }
|
|
276 |
// whiteBuf = 0;
|
|
277 |
// proxyBuf = 0;
|
| 255 |
278 |
// lf.followLine(rpmSpeed);
|
| 256 |
|
// setRpmSpeed(rpmSpeed);
|
|
279 |
setRpmSpeed(rpmSpeed);
|
| 257 |
280 |
|
| 258 |
281 |
break;
|
| 259 |
282 |
// ---------------------------------------
|
| 260 |
283 |
case states::DETECT_STATION:
|
| 261 |
|
// if (global.forwardSpeed != CHARGING_SPEED){
|
| 262 |
|
// global.forwardSpeed = CHARGING_SPEED;
|
| 263 |
|
// }
|
|
284 |
if (global.forwardSpeed != DETECTION_SPEED){
|
|
285 |
global.forwardSpeed = DETECTION_SPEED;
|
|
286 |
}
|
| 264 |
287 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
| 265 |
288 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
|
| 266 |
289 |
}
|
| ... | ... | |
| 312 |
335 |
// Is of those sensors at it max val means that the AMiRo cant drive back
|
| 313 |
336 |
// so check if correctly positioned
|
| 314 |
337 |
//definitely wrong positioned -> correct position directly without rotation
|
| 315 |
|
if ((rProx[0] >= PROX_MAX_VAL && rProx[7] < PROX_MAX_VAL) || (rProx[0] < PROX_MAX_VAL && rProx[7] >= PROX_MAX_VAL)){
|
| 316 |
|
setRpmSpeed(stop);
|
| 317 |
|
checkForMotion();
|
| 318 |
|
newState = states::CORRECT_POSITIONING;
|
| 319 |
|
}
|
| 320 |
338 |
// Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
|
|
339 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
|
340 |
// setRpmSpeed(stop);
|
|
341 |
// checkForMotion();
|
|
342 |
// newState = states::CHECK_POSITIONING;
|
|
343 |
// } else
|
| 321 |
344 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
|
| 322 |
|
setRpmSpeed(stop);
|
| 323 |
|
checkForMotion();
|
|
345 |
// setRpmSpeed(stop);
|
|
346 |
// checkForMotion();
|
| 324 |
347 |
newState = states::CHECK_POSITIONING;
|
| 325 |
348 |
}
|
| 326 |
349 |
break;
|
| 327 |
350 |
// ---------------------------------------
|
| 328 |
351 |
case states::CHECK_POSITIONING:
|
| 329 |
|
if(checkDockingSuccess()){
|
| 330 |
|
newState = states::CHECK_VOLTAGE;
|
| 331 |
|
}else{
|
| 332 |
|
newState = states::CORRECT_POSITIONING;
|
| 333 |
|
}
|
|
352 |
setRpmSpeed(stop);
|
|
353 |
checkForMotion();
|
|
354 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
|
355 |
if(checkDockingSuccess()){
|
|
356 |
newState = states::CHECK_VOLTAGE;
|
|
357 |
}else{
|
|
358 |
newState = states::CORRECT_POSITIONING;
|
|
359 |
}
|
|
360 |
// }
|
|
361 |
// else{
|
|
362 |
// newState = CORRECT_POSITIONING;
|
|
363 |
// }
|
| 334 |
364 |
break;
|
| 335 |
365 |
// ---------------------------------------
|
| 336 |
366 |
case states::CHECK_VOLTAGE:
|
| ... | ... | |
| 344 |
374 |
// No voltage on pins -> falsely docked
|
| 345 |
375 |
// deactivate pins
|
| 346 |
376 |
global.robot.requestCharging(0);
|
| 347 |
|
newState = states::RELEASE_TO_CORRECT;
|
|
377 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
|
378 |
newState = states::RELEASE_TO_CORRECT;
|
|
379 |
} else {
|
|
380 |
newState = states::CORRECT_POSITIONING;
|
|
381 |
}
|
| 348 |
382 |
}
|
| 349 |
383 |
}
|
| 350 |
384 |
break;
|
| ... | ... | |
| 379 |
413 |
|
| 380 |
414 |
// ---------------------------------------
|
| 381 |
415 |
case states::RELEASE:
|
|
416 |
if (global.forwardSpeed != DETECTION_SPEED){
|
|
417 |
global.rpmForward[0] = DETECTION_SPEED;
|
|
418 |
}
|
| 382 |
419 |
if(/* checkPinVoltage() && */ checkPinEnabled()){
|
| 383 |
420 |
global.robot.requestCharging(0);
|
| 384 |
|
}
|
| 385 |
|
|
| 386 |
|
if(checkPinEnabled()){
|
| 387 |
|
showChargingState();
|
| 388 |
421 |
}else{
|
| 389 |
422 |
if (!global.motorcontrol.getMotorEnable()){
|
| 390 |
423 |
global.motorcontrol.toggleMotorEnable();
|
| 391 |
424 |
}
|
|
425 |
// if (releaseBuf > RELEASE_COUNT){
|
|
426 |
// releaseBuf = 0;
|
|
427 |
// setRpmSpeed(stop);
|
|
428 |
// needDistance = true;
|
|
429 |
// newState = states::FOLLOW_LINE;
|
|
430 |
// }else{
|
|
431 |
// releaseBuf++;
|
|
432 |
// setRpmSpeed(turn);
|
|
433 |
// }
|
| 392 |
434 |
//Rotate -20° to free from magnet
|
| 393 |
435 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
| 394 |
|
checkForMotion();
|
| 395 |
|
// move 1cm forward
|
| 396 |
|
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
| 397 |
|
checkForMotion();
|
| 398 |
|
// rotate back
|
| 399 |
|
global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
| 400 |
|
checkForMotion();
|
|
436 |
checkForMotion();
|
|
437 |
// move 1cm forward
|
|
438 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
|
439 |
checkForMotion();
|
|
440 |
// rotate back
|
|
441 |
global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
|
442 |
checkForMotion();
|
| 401 |
443 |
|
| 402 |
|
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
| 403 |
|
checkForMotion();
|
|
444 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
|
445 |
// checkForMotion();
|
| 404 |
446 |
lStrategy = LineFollowStrategy::EDGE_RIGHT;
|
| 405 |
447 |
newState = states::FOLLOW_LINE;
|
| 406 |
|
whiteBuf = -100;
|
|
448 |
// whiteBuf = -100;
|
|
449 |
// lf.followLine(rpmSpeed);
|
|
450 |
// setRpmSpeed(rpmSpeed);
|
| 407 |
451 |
}
|
| 408 |
|
lightAllLeds(Color::BLACK);
|
|
452 |
// lightAllLeds(Color::BLACK);
|
| 409 |
453 |
break;
|
| 410 |
454 |
|
| 411 |
455 |
default:
|