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