Revision d4c6efa9
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 169 | 169 |
int rpmHardLeft[2] = {5,20};
|
| 170 | 170 |
int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
|
| 171 | 171 |
int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
|
| 172 |
|
|
| 172 |
uint32_t stateTracker[23] = { 0 };
|
|
| 173 | 173 |
|
| 174 | 174 |
// Line Following thresholds set due to calibration |
| 175 | 175 |
// MaxDelta: 18676, FL: 4289, FR: 22965 |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 908 | 908 |
chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y); |
| 909 | 909 |
} |
| 910 | 910 |
|
| 911 |
|
|
| 912 |
void shellRequestErrorInfo(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 913 |
// Print out the error info collected. Clear buffer after calling |
|
| 914 |
chprintf(chp, "Error Info\n"); |
|
| 915 |
chprintf(chp, "\n"); |
|
| 916 |
int IDLE = 0; |
|
| 917 |
int FOLLOW_LINE = 1; |
|
| 918 |
int DETECT_STATION = 2; |
|
| 919 |
int REVERSE = 3; |
|
| 920 |
int PUSH_BACK = 4; |
|
| 921 |
int CHECK_POSITIONING = 5; |
|
| 922 |
int CHECK_VOLTAGE = 6; |
|
| 923 |
int CHARGING = 7; |
|
| 924 |
int RELEASE = 8; |
|
| 925 |
int RELEASE_TO_CORRECT = 9; |
|
| 926 |
int CORRECT_POSITIONING = 10; |
|
| 927 |
int TURN = 12; |
|
| 928 |
int INACTIVE = 13; |
|
| 929 |
int CALIBRATION = 14; |
|
| 930 |
int CALIBRATION_CHECK = 15; |
|
| 931 |
int DEVIATION_CORRECTION = 16; |
|
| 932 |
int DOCKING_ERROR = 16+1; |
|
| 933 |
int REVERSE_TIMEOUT_ERROR = 16+2; |
|
| 934 |
int CALIBRATION_ERROR = 16+3; |
|
| 935 |
int WHITE_DETECTION_ERROR = 16+4; |
|
| 936 |
int PROXY_DETECTION_ERROR = 16+5; |
|
| 937 |
int NO_CHARGING_POWER_ERROR = 16+6; |
|
| 938 |
int UNKNOWN_STATE_ERROR = 16+7; |
|
| 939 |
|
|
| 940 |
chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]); |
|
| 941 |
chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]); |
|
| 942 |
chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]); |
|
| 943 |
chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]); |
|
| 944 |
chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]); |
|
| 945 |
chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]); |
|
| 946 |
chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]); |
|
| 947 |
chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]); |
|
| 948 |
chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]); |
|
| 949 |
chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]); |
|
| 950 |
chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]); |
|
| 951 |
chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]); |
|
| 952 |
chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]); |
|
| 953 |
chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]); |
|
| 954 |
chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]); |
|
| 955 |
chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]); |
|
| 956 |
chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]); |
|
| 957 |
chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]); |
|
| 958 |
chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]); |
|
| 959 |
chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]); |
|
| 960 |
chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]); |
|
| 961 |
chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]); |
|
| 962 |
chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]); |
|
| 963 |
|
|
| 964 |
for (int i=0; i<24;i++){
|
|
| 965 |
global.stateTracker[i] = 0; |
|
| 966 |
} |
|
| 967 |
|
|
| 968 |
} |
|
| 969 |
|
|
| 911 | 970 |
static const ShellCommand commands[] = {
|
| 912 | 971 |
{"shutdown", shellRequestShutdown},
|
| 913 | 972 |
{"wakeup", shellRequestWakeup},
|
| ... | ... | |
| 941 | 1000 |
{"printMagnetometerRes", shellRequestMagnetoMeterPrint},
|
| 942 | 1001 |
{"printLocation", shellRequestPrintCoordinate},
|
| 943 | 1002 |
{"checkPowerPins", shellRequestCheckPower},
|
| 1003 |
{"infos", shellRequestErrorInfo},
|
|
| 944 | 1004 |
{NULL, NULL}
|
| 945 | 1005 |
}; |
| 946 | 1006 |
|
| 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; |
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 8 | 8 |
// #include "linefollow.hpp" |
| 9 | 9 |
#include <cmath> |
| 10 | 10 |
|
| 11 |
|
|
| 12 |
|
|
| 13 |
// TODO: Clean up and sort defines! |
|
| 11 | 14 |
// Speed when driving towards the docking station |
| 12 | 15 |
#define CHARGING_SPEED 5 |
| 13 | 16 |
#define DETECTION_SPEED 10 |
| 14 | 17 |
#define DIST_THRESH 100 |
| 15 | 18 |
#define RELEASE_COUNT 200 |
| 19 |
#define SPEED_CONVERSION_FACTOR 1000000 //Convert value to um/s |
|
| 16 | 20 |
// Thresh to determain how much update steps should pass while alining |
| 17 | 21 |
// #define MAX_CORRECTION_STEPS 200 |
| 18 | 22 |
#define DOCKING_CORRECTION_TIMEOUT 200 |
Also available in: Unified diff