Revision 84b4c632 devices/DiWheelDrive/userthread.cpp
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 210 | 210 |
|
| 211 | 211 |
// Amiro moved, docking was not successful |
| 212 | 212 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){
|
| 213 |
if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){
|
|
| 213 |
if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){
|
|
| 214 | 214 |
lightAllLeds(Color::RED); |
| 215 | 215 |
// Enable Motor again if docking was not successful |
| 216 | 216 |
global.motorcontrol.setMotorEnable(true); |
| ... | ... | |
| 544 | 544 |
} |
| 545 | 545 |
} |
| 546 | 546 |
|
| 547 |
// if((devCor.currentDeviation <= -10)){
|
|
| 548 |
// rpmSpeed[0] -= 2000000; |
|
| 549 |
// }else if(devCor.currentDeviation >= 10){
|
|
| 550 |
// rpmSpeed[1] -= 2000000; |
|
| 551 |
// } |
|
| 552 |
// setRpmSpeed(rpmSpeed); |
|
| 547 | 553 |
break; |
| 548 | 554 |
// --------------------------------------- |
| 549 | 555 |
case states::DEVIATION_CORRECTION: |
| ... | ... | |
| 561 | 567 |
rpmSpeed[1] = 0; |
| 562 | 568 |
} |
| 563 | 569 |
setRpmSpeed(rpmSpeed); |
| 564 |
}else if ((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION)){
|
|
| 570 |
}else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){
|
|
| 565 | 571 |
if(devCor.RCase){
|
| 566 | 572 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED; |
| 567 | 573 |
rpmSpeed[1] = 0; |
| ... | ... | |
| 570 | 576 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED; |
| 571 | 577 |
} |
| 572 | 578 |
setRpmSpeed(rpmSpeed); |
| 573 |
}else if (utCount.stateTime >= DEVIATION_CORRECTION_DURATION + 10) {
|
|
| 574 |
// Wait to clear the mean window buffer |
|
| 575 |
setRpmSpeed(stop); |
|
| 579 |
if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
|
|
| 580 |
utCount.stateTime = 0; |
|
| 581 |
newState = states::REVERSE; |
|
| 582 |
setRpmSpeed(stop); |
|
| 583 |
} |
|
| 576 | 584 |
}else{
|
| 577 | 585 |
utCount.stateTime = 0; |
| 578 | 586 |
newState = states::REVERSE; |
| ... | ... | |
| 740 | 748 |
} |
| 741 | 749 |
prevState = currentState; |
| 742 | 750 |
currentState = newState; |
| 743 |
if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
|
| 744 |
utCount.stateCount = 0; |
|
| 745 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
| 746 |
global.robot.transmitState(currentState); |
|
| 747 |
// global.robot.setOdometry(global.odometry.getPosition()); |
|
| 751 |
// if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
|
| 752 |
// utCount.stateCount = 0;
|
|
| 753 |
// // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
|
| 754 |
// global.robot.transmitState(currentState);
|
|
| 755 |
// // global.robot.setOdometry(global.odometry.getPosition());
|
|
| 748 | 756 |
|
| 749 |
}else{
|
|
| 750 |
utCount.stateCount++; |
|
| 751 |
} |
|
| 757 |
// }else{
|
|
| 758 |
// utCount.stateCount++;
|
|
| 759 |
// }
|
|
| 752 | 760 |
this->sleep(CAN::UPDATE_PERIOD); |
| 753 | 761 |
} |
| 754 | 762 |
|
Also available in: Unified diff