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