Revision b24df8ad
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 233 | 233 |
return prox_sum; |
| 234 | 234 |
} |
| 235 | 235 |
|
| 236 |
int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){
|
|
| 237 |
int32_t diff = a - b; |
|
| 238 |
int32_t res = 0; |
|
| 239 |
devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2); |
|
| 240 |
for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){
|
|
| 241 |
res += devCor.proxbuf[i]; |
|
| 242 |
} |
|
| 243 |
devCor.pCount++; |
|
| 244 |
devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW; |
|
| 245 |
|
|
| 246 |
devCor.currentDeviation = res / PROX_DEVIATION_MEAN_WINDOW; |
|
| 247 |
return devCor.currentDeviation; |
|
| 248 |
} |
|
| 249 |
|
|
| 236 | 250 |
|
| 237 | 251 |
UserThread::UserThread() : |
| 238 | 252 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
| ... | ... | |
| 335 | 349 |
for(int i=0; i<8;i++){
|
| 336 | 350 |
rProx[i] = global.robot.getProximityRingValue(i); |
| 337 | 351 |
} |
| 352 |
|
|
| 353 |
// Continously update devication values |
|
| 354 |
meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0); |
|
| 338 | 355 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
| 339 | 356 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
| 340 | 357 |
switch(currentState){
|
| ... | ... | |
| 494 | 511 |
} |
| 495 | 512 |
lf.followLine(rpmSpeed); |
| 496 | 513 |
setRpmSpeed(rpmSpeed); |
| 497 |
utCount.stateTime++; |
|
| 514 |
// utCount.stateTime++;
|
|
| 498 | 515 |
|
| 499 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
|
|
| 516 |
// 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) )){
|
|
| 500 | 518 |
// setRpmSpeed(stop); |
| 501 | 519 |
// checkForMotion(); |
| 502 | 520 |
utCount.stateTime = 0; |
| 503 | 521 |
newState = states::PUSH_BACK; |
| 522 |
}else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
|
|
| 523 |
// Case R |
|
| 524 |
utCount.stateTime = 0; |
|
| 525 |
setRpmSpeed(stop); |
|
| 526 |
devCor.RCase = true; |
|
| 527 |
lightAllLeds(Color::YELLOW); |
|
| 528 |
newState = states::DEVIATION_CORRECTION; |
|
| 529 |
}else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
|
|
| 530 |
// Case L |
|
| 531 |
utCount.stateTime = 0; |
|
| 532 |
setRpmSpeed(stop); |
|
| 533 |
devCor.RCase = false; |
|
| 534 |
lightAllLeds(Color::WHITE); |
|
| 535 |
newState = states::DEVIATION_CORRECTION; |
|
| 504 | 536 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
|
| 505 | 537 |
setRpmSpeed(stop); |
| 506 | 538 |
utCount.stateTime = 0; |
| ... | ... | |
| 514 | 546 |
|
| 515 | 547 |
break; |
| 516 | 548 |
// --------------------------------------- |
| 549 |
case states::DEVIATION_CORRECTION: |
|
| 550 |
// if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
|
| 551 |
// lf.setStrategy(LineFollowStrategy::REVERSE); |
|
| 552 |
// } |
|
| 553 |
// lf.followLine(rpmSpeed); |
|
| 554 |
// setRpmSpeed(rpmSpeed); |
|
| 555 |
if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){
|
|
| 556 |
if(devCor.RCase){
|
|
| 557 |
rpmSpeed[0] = 0; |
|
| 558 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED; |
|
| 559 |
}else {
|
|
| 560 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED; |
|
| 561 |
rpmSpeed[1] = 0; |
|
| 562 |
} |
|
| 563 |
setRpmSpeed(rpmSpeed); |
|
| 564 |
}else if ((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION)){
|
|
| 565 |
if(devCor.RCase){
|
|
| 566 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED; |
|
| 567 |
rpmSpeed[1] = 0; |
|
| 568 |
}else {
|
|
| 569 |
rpmSpeed[0] = 0; |
|
| 570 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED; |
|
| 571 |
} |
|
| 572 |
setRpmSpeed(rpmSpeed); |
|
| 573 |
}else if (utCount.stateTime >= DEVIATION_CORRECTION_DURATION + 10) {
|
|
| 574 |
// Wait to clear the mean window buffer |
|
| 575 |
setRpmSpeed(stop); |
|
| 576 |
}else{
|
|
| 577 |
utCount.stateTime = 0; |
|
| 578 |
newState = states::REVERSE; |
|
| 579 |
setRpmSpeed(stop); |
|
| 580 |
} |
|
| 581 |
|
|
| 582 |
utCount.stateTime++; |
|
| 583 |
|
|
| 584 |
|
|
| 585 |
// if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
|
| 586 |
// utCount.stateTime = 0; |
|
| 587 |
// newState = states::CHECK_POSITIONING; |
|
| 588 |
// } |
|
| 589 |
break; |
|
| 590 |
// --------------------------------------- |
|
| 517 | 591 |
case states::PUSH_BACK: |
| 518 | 592 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
| 519 | 593 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 17 | 17 |
// #define MAX_CORRECTION_STEPS 200 |
| 18 | 18 |
#define DOCKING_CORRECTION_TIMEOUT 200 |
| 19 | 19 |
#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT |
| 20 |
#define REVERSE_ADJUSTMENT_TIMEOUT 200 |
|
| 21 |
// #define MAX_RING_PROX_VALUE_DEVIATION |
|
| 20 | 22 |
|
| 21 | 23 |
// Thresh for wheel proxy sensors, when summed values fall below the state changes |
| 22 | 24 |
// #define PROXY_WHEEL_THRESH 18000 |
| ... | ... | |
| 38 | 40 |
#define ROTATION_DURATION 10000 |
| 39 | 41 |
|
| 40 | 42 |
#define RING_PROX_FRONT_THRESH 18000 |
| 41 |
#define PROX_MAX_VAL 65430 |
|
| 43 |
// #define PROX_MAX_VAL 65430 |
|
| 44 |
#define PROX_MAX_VAL 0xFFF0 |
|
| 45 |
|
|
| 42 | 46 |
|
| 43 | 47 |
// Threshold for failing to dock |
| 44 | 48 |
#define DOCKING_ERROR_THRESH 3 |
| 45 | 49 |
#define CAN_TRANSMIT_STATE_THRESH 50 |
| 46 |
|
|
| 50 |
#define PROX_DEVIATION_MEAN_WINDOW 5 |
|
| 51 |
#define MAX_DEVIATION_CORRECTIONS 4 |
|
| 52 |
#define MAX_DEVIATION_FACTOR 45 |
|
| 53 |
#define DEVIATION_CORRECTION_DURATION 1000 |
|
| 54 |
#define DEVIATION_CORRECTION_SPEED 2000000 |
|
| 55 |
#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2) |
|
| 56 |
#define DEVIATION_DIST_THRESH 6000 |
|
| 47 | 57 |
|
| 48 | 58 |
namespace amiro {
|
| 49 | 59 |
|
| ... | ... | |
| 71 | 81 |
INACTIVE = 13, |
| 72 | 82 |
CALIBRATION = 14, |
| 73 | 83 |
CALIBRATION_CHECK = 15, |
| 84 |
DEVIATION_CORRECTION = 16, |
|
| 74 | 85 |
DOCKING_ERROR = -1, |
| 75 | 86 |
REVERSE_TIMEOUT_ERROR = -2, |
| 76 | 87 |
CALIBRATION_ERROR = -3, |
| ... | ... | |
| 107 | 118 |
uint8_t meanWindow = 150; |
| 108 | 119 |
}; |
| 109 | 120 |
|
| 121 |
struct deviation_correction {
|
|
| 122 |
bool RCase = true; |
|
| 123 |
int8_t pCount = 0; |
|
| 124 |
int32_t proxbuf[PROX_DEVIATION_MEAN_WINDOW] = { 0 };
|
|
| 125 |
int32_t currentDeviation = 0; |
|
| 126 |
}; |
|
| 127 |
|
|
| 110 | 128 |
// static const struct ut_counter emptyUtCount; |
| 111 | 129 |
ut_counter utCount; |
| 112 | 130 |
proxy_ctrl pCtrl; |
| 113 | 131 |
bottom_prox_calibration proxCalib; |
| 132 |
deviation_correction devCor; |
|
| 133 |
|
|
| 134 |
|
|
| 114 | 135 |
explicit UserThread(); |
| 115 | 136 |
|
| 116 | 137 |
virtual ~UserThread(); |
| ... | ... | |
| 132 | 153 |
int getProxyRingSum(); |
| 133 | 154 |
|
| 134 | 155 |
/** |
| 156 |
* Returns percentage of mean deviation between two given values. |
|
| 157 |
* It is intended to calculate the mean deviation between two proxy sensor |
|
| 158 |
* values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window. |
|
| 159 |
* Keep in mind that initial results are wrong. |
|
| 160 |
* */ |
|
| 161 |
int32_t meanDeviation(uint16_t a, uint16_t b); |
|
| 162 |
|
|
| 163 |
/** |
|
| 135 | 164 |
* Check sectors around and stop if a thresh in one sector is detected. |
| 136 | 165 |
*/ |
| 137 | 166 |
void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]); |
Also available in: Unified diff