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;
|