Revision b24df8ad devices/DiWheelDrive/userthread.cpp
| 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); |
Also available in: Unified diff