Revision d4c6efa9 devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
2 | 2 |
#include "global.hpp" |
3 | 3 |
#include <cmath> |
4 | 4 |
#include "linefollow.hpp" |
5 |
#include "userthread.hpp" |
|
5 | 6 |
// #include <cmath> |
6 | 7 |
// #include "global.hpp" |
7 | 8 |
using namespace amiro; |
... | ... | |
138 | 139 |
} |
139 | 140 |
|
140 | 141 |
if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){ |
141 |
rpmSpeed[0] = rpmSpeed[0] / 4;
|
|
142 |
rpmSpeed[1] = rpmSpeed[1] / 4;
|
|
142 |
rpmSpeed[0] = rpmSpeed[0] / 3;
|
|
143 |
rpmSpeed[1] = rpmSpeed[1] / 3;
|
|
143 | 144 |
} |
144 | 145 |
|
145 | 146 |
if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){ |
... | ... | |
435 | 436 |
|
436 | 437 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){ |
437 | 438 |
utCount.ringProxCount = 0; |
438 |
newState = states::TURN; |
|
439 |
|
|
440 |
|
|
441 |
checkForMotion(); |
|
442 |
// Check if only front sensors are active |
|
443 |
if (checkFrontalObject()) { |
|
444 |
// global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION); |
|
445 |
// // BaseThread::sleep(8000); |
|
446 |
// checkForMotion(); |
|
447 |
this->utCount.whiteCount = 0; |
|
448 |
newState = states::TURN; |
|
449 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
|
450 |
} else { |
|
451 |
newState = states::PROXY_DETECTION_ERROR; |
|
452 |
} |
|
439 | 453 |
} |
440 | 454 |
|
441 | 455 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){ |
... | ... | |
447 | 461 |
|
448 | 462 |
break; |
449 | 463 |
// --------------------------------------- |
450 |
case states::TURN: |
|
451 |
checkForMotion(); |
|
452 |
// Check if only front sensors are active |
|
453 |
if(checkFrontalObject()){ |
|
454 |
global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION); |
|
455 |
// BaseThread::sleep(8000); |
|
456 |
checkForMotion(); |
|
464 |
case states::TURN:{ |
|
465 |
// Check the line strategy in order to continue driving on the right side |
|
466 |
int factor = SPEED_CONVERSION_FACTOR; |
|
467 |
int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
468 |
int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
469 |
if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) { |
|
470 |
factor = -factor; |
|
471 |
} |
|
472 |
|
|
473 |
rpmSpeed[0] = factor * CHARGING_SPEED; |
|
474 |
rpmSpeed[1] = -factor * CHARGING_SPEED; |
|
475 |
setRpmSpeed(rpmSpeed); |
|
476 |
|
|
477 |
if ((frontL > 2* pCtrl.threshHigh ) && (frontR > 2* pCtrl.threshHigh )){ |
|
478 |
utCount.whiteCount = 1; |
|
479 |
}else{ |
|
480 |
if(utCount.whiteCount){ |
|
481 |
utCount.whiteCount = 0; |
|
457 | 482 |
newState = states::FOLLOW_LINE; |
458 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
|
459 |
}else{ |
|
460 |
newState = states::PROXY_DETECTION_ERROR; |
|
483 |
setRpmSpeed(stop); |
|
461 | 484 |
} |
485 |
} |
|
462 | 486 |
break; |
487 |
} |
|
463 | 488 |
// --------------------------------------- |
464 | 489 |
case states::DETECT_STATION: |
490 |
|
|
465 | 491 |
if (global.forwardSpeed != DETECTION_SPEED){ |
466 | 492 |
global.forwardSpeed = DETECTION_SPEED; |
467 | 493 |
} |
468 | 494 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){ |
469 | 495 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
470 | 496 |
} |
471 |
|
|
497 |
|
|
472 | 498 |
lf.followLine(rpmSpeed); |
473 | 499 |
setRpmSpeed(rpmSpeed); |
474 | 500 |
// // Detect marker before docking station |
... | ... | |
514 | 540 |
// utCount.stateTime++; |
515 | 541 |
|
516 | 542 |
// Docking is only successful if Deviation is in range and sensors are at their max values. |
517 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL) && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){ |
|
543 |
if((rProx[0] >= PROX_MAX_VAL) |
|
544 |
&& (rProx[7] >= PROX_MAX_VAL) |
|
545 |
&& ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){ |
|
518 | 546 |
// setRpmSpeed(stop); |
519 | 547 |
// checkForMotion(); |
520 | 548 |
utCount.stateTime = 0; |
... | ... | |
653 | 681 |
break; |
654 | 682 |
// --------------------------------------- |
655 | 683 |
case states::RELEASE_TO_CORRECT: |
656 |
|
|
684 |
|
|
657 | 685 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
658 | 686 |
checkForMotion(); |
659 | 687 |
// move 1cm forward |
... | ... | |
688 | 716 |
global.robot.requestCharging(0); |
689 | 717 |
}else{ |
690 | 718 |
global.motorcontrol.setMotorEnable(true); |
691 |
|
|
719 |
// TODO: Use controlled |
|
692 | 720 |
//Rotate -20° to free from magnet |
693 | 721 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
694 | 722 |
checkForMotion(); |
... | ... | |
745 | 773 |
if (currentState != newState){ |
746 | 774 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
747 | 775 |
global.robot.transmitState(newState); |
776 |
if (newState == states::IDLE) |
|
777 |
{global.stateTracker[states::IDLE] += 1;} |
|
778 |
else if (newState == states::FOLLOW_LINE) |
|
779 |
{global.stateTracker[states::FOLLOW_LINE] += 1;} |
|
780 |
else if (newState == states::DETECT_STATION) |
|
781 |
{global.stateTracker[states::DETECT_STATION] += 1;} |
|
782 |
else if (newState == states::REVERSE) |
|
783 |
{global.stateTracker[states::REVERSE] += 1;} |
|
784 |
else if (newState == states::PUSH_BACK) |
|
785 |
{global.stateTracker[states::PUSH_BACK] += 1;} |
|
786 |
else if (newState == states::CHECK_POSITIONING) |
|
787 |
{global.stateTracker[states::CHECK_POSITIONING] += 1;} |
|
788 |
else if (newState == states::CHECK_VOLTAGE) |
|
789 |
{global.stateTracker[states::CHECK_VOLTAGE] += 1;} |
|
790 |
else if (newState == states::CHARGING) |
|
791 |
{global.stateTracker[states::CHARGING] += 1;} |
|
792 |
else if (newState == states::RELEASE) |
|
793 |
{global.stateTracker[states::RELEASE] += 1;} |
|
794 |
else if (newState == states::RELEASE_TO_CORRECT) |
|
795 |
{global.stateTracker[states::RELEASE_TO_CORRECT] += 1;} |
|
796 |
else if (newState == states::CORRECT_POSITIONING) |
|
797 |
{global.stateTracker[states::CORRECT_POSITIONING] += 1;} |
|
798 |
else if (newState == states::TURN) |
|
799 |
{global.stateTracker[states::TURN] += 1;} |
|
800 |
else if (newState == states::INACTIVE) |
|
801 |
{global.stateTracker[states::INACTIVE] += 1;} |
|
802 |
else if (newState == states::CALIBRATION) |
|
803 |
{global.stateTracker[states::CALIBRATION] += 1;} |
|
804 |
else if (newState == states::CALIBRATION_CHECK) |
|
805 |
{global.stateTracker[states::CALIBRATION_CHECK] += 1;} |
|
806 |
else if (newState == states::DEVIATION_CORRECTION) |
|
807 |
{global.stateTracker[states::DEVIATION_CORRECTION] += 1;} |
|
808 |
|
|
809 |
else if (newState == states::DOCKING_ERROR){global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;} |
|
810 |
else if (newState == states::REVERSE_TIMEOUT_ERROR){global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;} |
|
811 |
else if (newState == states::CALIBRATION_ERROR){global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;} |
|
812 |
else if (newState == states::WHITE_DETECTION_ERROR){global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;} |
|
813 |
else if (newState == states::PROXY_DETECTION_ERROR){global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;} |
|
814 |
else if (newState == states::NO_CHARGING_POWER_ERROR){global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;} |
|
815 |
else if (newState == states::UNKNOWN_STATE_ERROR){global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;} |
|
748 | 816 |
} |
749 | 817 |
prevState = currentState; |
750 | 818 |
currentState = newState; |
Also available in: Unified diff