Revision 27d4e1fa devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
83 | 83 |
int success = 0; |
84 | 84 |
global.odometry.resetPosition(); |
85 | 85 |
types::position start = global.startPos = global.odometry.getPosition(); |
86 |
global.motorcontrol.toggleMotorEnable();
|
|
87 |
this->sleep(500);
|
|
86 |
global.motorcontrol.setMotorEnable(false);
|
|
87 |
this->sleep(1000);
|
|
88 | 88 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
89 | 89 |
|
90 | 90 |
// Amiro moved, docking was not successful |
91 | 91 |
if ((start.x + stop_.x) || (start.y + stop_.y)){ |
92 | 92 |
lightAllLeds(Color::RED); |
93 | 93 |
// Enable Motor again if docking was not successful |
94 |
global.motorcontrol.toggleMotorEnable();
|
|
94 |
global.motorcontrol.setMotorEnable(true);
|
|
95 | 95 |
success = 0; |
96 | 96 |
}else{ |
97 | 97 |
lightAllLeds(Color::GREEN); |
... | ... | |
133 | 133 |
|
134 | 134 |
int whiteBuf = 0; |
135 | 135 |
int proxyBuf = 0; |
136 |
int releaseBuf = 0;
|
|
136 |
// int reverseBuf = 0;
|
|
137 | 137 |
int correctionStep = 0; |
138 |
int dist_count = 0; |
|
139 |
bool needDistance = false; |
|
138 |
// int dist_count = 0;
|
|
139 |
// bool needDistance = false;
|
|
140 | 140 |
|
141 | 141 |
uint16_t rProx[8]; // buffer for ring proxy values |
142 | 142 |
int rpmSpeed[2] = {0}; |
... | ... | |
218 | 218 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
219 | 219 |
switch(utState){ |
220 | 220 |
case states::IDLE: |
221 |
if (!global.motorcontrol.getMotorEnable()){ |
|
222 |
global.motorcontrol.toggleMotorEnable(); |
|
223 |
} |
|
221 |
global.motorcontrol.setMotorEnable(true); |
|
224 | 222 |
setRpmSpeed(stop); |
225 | 223 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
226 | 224 |
global.robot.requestCharging(0); |
227 | 225 |
} |
228 | 226 |
whiteBuf = 0; |
229 | 227 |
proxyBuf = 0; |
228 |
utCount.errorCount = 0; |
|
230 | 229 |
break; |
231 | 230 |
// --------------------------------------- |
232 | 231 |
case states::FOLLOW_LINE: |
... | ... | |
353 | 352 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
354 | 353 |
// setRpmSpeed(stop); |
355 | 354 |
// checkForMotion(); |
355 |
utCount.reverseCount = 0; |
|
356 |
newState = states::PUSH_BACK; |
|
357 |
} |
|
358 |
break; |
|
359 |
// --------------------------------------- |
|
360 |
case states::PUSH_BACK: |
|
361 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){ |
|
362 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
|
363 |
} |
|
364 |
lf.followLine(rpmSpeed); |
|
365 |
setRpmSpeed(rpmSpeed); |
|
366 |
|
|
367 |
utCount.reverseCount++; |
|
368 |
if (utCount.reverseCount > PUSH_BACK_COUNT){ |
|
356 | 369 |
newState = states::CHECK_POSITIONING; |
357 | 370 |
} |
358 | 371 |
break; |
... | ... | |
361 | 374 |
setRpmSpeed(stop); |
362 | 375 |
checkForMotion(); |
363 | 376 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
364 |
if(checkDockingSuccess()){ |
|
365 |
newState = states::CHECK_VOLTAGE; |
|
366 |
}else{ |
|
367 |
newState = states::CORRECT_POSITIONING; |
|
368 |
} |
|
377 |
if(checkDockingSuccess()){ |
|
378 |
newState = states::CHECK_VOLTAGE; |
|
379 |
}else{ |
|
380 |
utCount.errorCount++; |
|
381 |
newState = states::CORRECT_POSITIONING; |
|
382 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){ |
|
383 |
newState = states::ERROR; |
|
384 |
} |
|
385 |
} |
|
369 | 386 |
// } |
370 | 387 |
// else{ |
371 | 388 |
// newState = CORRECT_POSITIONING; |
... | ... | |
378 | 395 |
} else { |
379 | 396 |
if(checkPinVoltage()){ |
380 | 397 |
// Pins are under voltage -> correctly docked |
398 |
utCount.errorCount = 0; |
|
381 | 399 |
newState = states::CHARGING; |
382 | 400 |
}else{ |
401 |
utCount.errorCount++; |
|
383 | 402 |
// No voltage on pins -> falsely docked |
384 | 403 |
// deactivate pins |
385 |
if (!global.motorcontrol.getMotorEnable()){ |
|
386 |
global.motorcontrol.toggleMotorEnable(); |
|
387 |
} |
|
404 |
global.motorcontrol.setMotorEnable(true); |
|
388 | 405 |
global.robot.requestCharging(0); |
406 |
// TODO: Soft release when docking falsely |
|
389 | 407 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
390 | 408 |
newState = states::RELEASE_TO_CORRECT; |
391 | 409 |
} else { |
392 |
newState = states::CORRECT_POSITIONING; |
|
410 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; |
|
411 |
} |
|
412 |
|
|
413 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){ |
|
414 |
newState = states::ERROR; |
|
393 | 415 |
} |
394 | 416 |
} |
395 | 417 |
} |
396 | 418 |
break; |
397 | 419 |
// --------------------------------------- |
398 | 420 |
case states::RELEASE_TO_CORRECT: |
421 |
|
|
399 | 422 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
400 | 423 |
checkForMotion(); |
401 | 424 |
// move 1cm forward |
... | ... | |
410 | 433 |
newState = states::CORRECT_POSITIONING; |
411 | 434 |
break; |
412 | 435 |
// --------------------------------------- |
436 |
case states::ERROR: |
|
437 |
utCount.errorCount = 0; |
|
438 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
439 |
newState = states::FOLLOW_LINE; |
|
440 |
break; |
|
441 |
// --------------------------------------- |
|
413 | 442 |
case states::CHARGING: |
414 |
if (global.motorcontrol.getMotorEnable()){ |
|
415 |
global.motorcontrol.toggleMotorEnable(); |
|
416 |
} |
|
443 |
global.motorcontrol.setMotorEnable(false); |
|
417 | 444 |
// Formulate Request to enable charging |
418 | 445 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
419 | 446 |
global.robot.requestCharging(1); |
... | ... | |
431 | 458 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
432 | 459 |
global.robot.requestCharging(0); |
433 | 460 |
}else{ |
434 |
if (!global.motorcontrol.getMotorEnable()){ |
|
435 |
global.motorcontrol.toggleMotorEnable(); |
|
436 |
} |
|
437 |
// if (releaseBuf > RELEASE_COUNT){ |
|
438 |
// releaseBuf = 0; |
|
439 |
// setRpmSpeed(stop); |
|
440 |
// needDistance = true; |
|
441 |
// newState = states::FOLLOW_LINE; |
|
442 |
// }else{ |
|
443 |
// releaseBuf++; |
|
444 |
// setRpmSpeed(turn); |
|
445 |
// } |
|
461 |
global.motorcontrol.setMotorEnable(true); |
|
462 |
|
|
446 | 463 |
//Rotate -20° to free from magnet |
447 | 464 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
448 | 465 |
checkForMotion(); |
... | ... | |
467 | 484 |
default: |
468 | 485 |
break; |
469 | 486 |
} |
487 |
if (utState != newState){ |
|
488 |
global.robot.transmitState(newState); |
|
489 |
} |
|
470 | 490 |
utState = newState; |
471 | 491 |
this->sleep(CAN::UPDATE_PERIOD); |
472 | 492 |
} |
Also available in: Unified diff