Revision 8dced1c9 devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
1 | 1 |
// #include "userthread.hpp" |
2 | 2 |
#include "global.hpp" |
3 | 3 |
#include <cmath> |
4 |
#include "linefollow.hpp"
|
|
4 |
#include "linefollow.hpp" |
|
5 | 5 |
#include "userthread.hpp" |
6 | 6 |
// #include <cmath> |
7 | 7 |
// #include "global.hpp" |
... | ... | |
16 | 16 |
|
17 | 17 |
/** |
18 | 18 |
* Set speed. |
19 |
*
|
|
19 |
* |
|
20 | 20 |
* @param rpmSpeed speed for left and right wheel in rounds/min |
21 | 21 |
*/ |
22 | 22 |
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) { |
... | ... | |
76 | 76 |
for (int i=0; i<8; i++){ |
77 | 77 |
sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8]; |
78 | 78 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]); |
79 |
|
|
79 |
|
|
80 | 80 |
} |
81 | 81 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "\n"); |
82 | 82 |
|
... | ... | |
95 | 95 |
int32_t sPMax = 0; |
96 | 96 |
getProxySectorVals(proxVals, sProx); |
97 | 97 |
getMaxFrontSectorVal(sProx, sPMax); |
98 |
|
|
98 |
|
|
99 | 99 |
int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor); |
100 | 100 |
int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor); |
101 | 101 |
|
... | ... | |
123 | 123 |
} |
124 | 124 |
} |
125 | 125 |
chargeAsLED(); |
126 |
|
|
126 |
|
|
127 | 127 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax, pCtrl.pFactor, sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]); |
128 | 128 |
|
129 | 129 |
|
... | ... | |
150 | 150 |
}else{ |
151 | 151 |
utCount.ringProxCount = 0; |
152 | 152 |
} |
153 |
|
|
153 |
|
|
154 | 154 |
} |
155 | 155 |
|
156 | 156 |
|
... | ... | |
158 | 158 |
* Blocks as long as the position changes. |
159 | 159 |
*/ |
160 | 160 |
void UserThread::checkForMotion(){ |
161 |
int motion = 1;
|
|
161 |
bool motion = true;
|
|
162 | 162 |
int led = 0; |
163 | 163 |
types::position oldPos = global.odometry.getPosition(); |
164 | 164 |
while(motion){ |
165 |
this->sleep(500);
|
|
165 |
this->sleep(200);
|
|
166 | 166 |
types::position tmp = global.odometry.getPosition(); |
167 |
motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
|
167 |
motion = oldPos.x != tmp.x; //abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
|
|
168 | 168 |
oldPos = tmp; |
169 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
|
|
170 |
global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
|
171 |
led++;
|
|
169 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
|
170 |
global.robot.setLightColor(led % 8, Color(Color::BLACK)); |
|
171 |
led++; |
|
172 | 172 |
} |
173 | 173 |
lightAllLeds(Color::BLACK); |
174 | 174 |
} |
... | ... | |
192 | 192 |
} |
193 | 193 |
|
194 | 194 |
bool UserThread::checkPinVoltage(){ |
195 |
return global.ltc4412.isPluggedIn();
|
|
195 |
return global.ltc4412.isPluggedIn(); |
|
196 | 196 |
} |
197 | 197 |
|
198 | 198 |
bool UserThread::checkPinEnabled(){ |
... | ... | |
208 | 208 |
global.motorcontrol.setMotorEnable(false); |
209 | 209 |
this->sleep(1000); |
210 | 210 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
211 |
|
|
211 |
|
|
212 | 212 |
// Amiro moved, docking was not successful |
213 | 213 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){ |
214 | 214 |
if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){ |
... | ... | |
220 | 220 |
lightAllLeds(Color::GREEN); |
221 | 221 |
success = 1; |
222 | 222 |
} |
223 |
|
|
223 |
|
|
224 | 224 |
// this->sleep(500); |
225 | 225 |
lightAllLeds(Color::BLACK); |
226 | 226 |
return success; |
... | ... | |
236 | 236 |
|
237 | 237 |
int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){ |
238 | 238 |
int32_t diff = a - b; |
239 |
int32_t res = 0;
|
|
239 |
int32_t res = 0; |
|
240 | 240 |
devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2); |
241 | 241 |
for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){ |
242 | 242 |
res += devCor.proxbuf[i]; |
... | ... | |
281 | 281 |
* read accelerometer z-value |
282 | 282 |
*/ |
283 | 283 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
284 |
|
|
285 |
if (accel_z < -900 /*-0.9g*/) {
|
|
284 |
|
|
285 |
if (accel_z < -900 /*-0.9g*/) { |
|
286 | 286 |
// Start line following when AMiRo is rotated |
287 | 287 |
if(currentState == states::INACTIVE){ |
288 | 288 |
newState = states::FOLLOW_LINE; |
... | ... | |
344 | 344 |
} |
345 | 345 |
// newState = currentState; |
346 | 346 |
|
347 |
// Get sensor data
|
|
347 |
// Get sensor data |
|
348 | 348 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
349 | 349 |
// uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
350 | 350 |
for(int i=0; i<8;i++){ |
... | ... | |
380 | 380 |
global.robot.calibrate(); |
381 | 381 |
} |
382 | 382 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
383 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
|
|
384 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
|
383 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
|
384 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
385 | 385 |
this->sleep(CAN::UPDATE_PERIOD); |
386 | 386 |
} |
387 | 387 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow); |
388 |
|
|
388 |
|
|
389 | 389 |
if(proxCalib.calibrateBlack){ |
390 | 390 |
global.linePID.BThresh = proxCalib.buf; |
391 | 391 |
}else { |
... | ... | |
415 | 415 |
if (global.forwardSpeed != global.rpmForward[0]){ |
416 | 416 |
global.forwardSpeed = global.rpmForward[0]; |
417 | 417 |
} |
418 |
|
|
418 |
|
|
419 | 419 |
if(lf.getStrategy() != lStrategy){ |
420 | 420 |
lf.setStrategy(lStrategy); |
421 | 421 |
} |
... | ... | |
436 | 436 |
|
437 | 437 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){ |
438 | 438 |
utCount.ringProxCount = 0; |
439 |
|
|
439 |
|
|
440 | 440 |
|
441 | 441 |
checkForMotion(); |
442 | 442 |
// Check if only front sensors are active |
... | ... | |
458 | 458 |
|
459 | 459 |
setRpmSpeed(rpmSpeed); |
460 | 460 |
} |
461 |
|
|
461 |
|
|
462 | 462 |
break; |
463 | 463 |
// --------------------------------------- |
464 | 464 |
case states::TURN:{ |
... | ... | |
466 | 466 |
int factor = SPEED_CONVERSION_FACTOR; |
467 | 467 |
int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
468 | 468 |
int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
469 |
int blackSensor = 0; |
|
469 | 470 |
if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) { |
470 | 471 |
factor = -factor; |
472 |
blackSensor = frontL; |
|
473 |
}else{ |
|
474 |
blackSensor = frontR; |
|
471 | 475 |
} |
472 |
|
|
476 |
|
|
473 | 477 |
rpmSpeed[0] = factor * CHARGING_SPEED; |
474 | 478 |
rpmSpeed[1] = -factor * CHARGING_SPEED; |
475 | 479 |
setRpmSpeed(rpmSpeed); |
476 |
|
|
477 |
if ((frontL > 2* pCtrl.threshHigh ) && (frontR > 2* pCtrl.threshHigh )){
|
|
480 |
|
|
481 |
if ((blackSensor >= global.linePID.WThresh )){
|
|
478 | 482 |
utCount.whiteCount = 1; |
479 |
}else{ |
|
480 |
if(utCount.whiteCount){
|
|
483 |
}else {
|
|
484 |
if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){
|
|
481 | 485 |
utCount.whiteCount = 0; |
482 | 486 |
newState = states::FOLLOW_LINE; |
483 | 487 |
setRpmSpeed(stop); |
... | ... | |
487 | 491 |
} |
488 | 492 |
// --------------------------------------- |
489 | 493 |
case states::DETECT_STATION: |
490 |
|
|
494 |
|
|
491 | 495 |
if (global.forwardSpeed != DETECTION_SPEED){ |
492 | 496 |
global.forwardSpeed = DETECTION_SPEED; |
493 | 497 |
} |
494 | 498 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){ |
495 | 499 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
496 | 500 |
} |
497 |
|
|
501 |
|
|
498 | 502 |
lf.followLine(rpmSpeed); |
499 | 503 |
setRpmSpeed(rpmSpeed); |
500 | 504 |
// // Detect marker before docking station |
501 | 505 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){ |
502 |
// Use proxy ring
|
|
506 |
// Use proxy ring |
|
503 | 507 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
504 | 508 |
|
505 | 509 |
setRpmSpeed(stop); |
506 | 510 |
checkForMotion(); |
507 |
// 180° Rotation
|
|
511 |
// 180° Rotation |
|
508 | 512 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION); |
509 | 513 |
// BaseThread::sleep(8000); |
510 | 514 |
checkForMotion(); |
... | ... | |
548 | 552 |
utCount.stateTime = 0; |
549 | 553 |
newState = states::PUSH_BACK; |
550 | 554 |
}else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
551 |
// Case R
|
|
555 |
// Case R |
|
552 | 556 |
utCount.stateTime = 0; |
553 | 557 |
setRpmSpeed(stop); |
554 | 558 |
devCor.RCase = true; |
555 | 559 |
lightAllLeds(Color::YELLOW); |
556 | 560 |
newState = states::DEVIATION_CORRECTION; |
557 | 561 |
}else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
558 |
// Case L
|
|
562 |
// Case L |
|
559 | 563 |
utCount.stateTime = 0; |
560 | 564 |
setRpmSpeed(stop); |
561 | 565 |
devCor.RCase = false; |
... | ... | |
657 | 661 |
global.robot.requestCharging(1); |
658 | 662 |
} else { |
659 | 663 |
if(checkPinVoltage()){ |
660 |
// Pins are under voltage -> correctly docked
|
|
661 |
|
|
664 |
// Pins are under voltage -> correctly docked |
|
665 |
|
|
662 | 666 |
newState = states::CHARGING; |
663 | 667 |
}else{ |
664 | 668 |
utCount.errorCount++; |
... | ... | |
670 | 674 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
671 | 675 |
newState = states::RELEASE_TO_CORRECT; |
672 | 676 |
} else { |
673 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
|
|
677 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; |
|
674 | 678 |
} |
675 | 679 |
|
676 | 680 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){ |
... | ... | |
681 | 685 |
break; |
682 | 686 |
// --------------------------------------- |
683 | 687 |
case states::RELEASE_TO_CORRECT: |
684 |
|
|
688 |
|
|
685 | 689 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
686 | 690 |
checkForMotion(); |
687 | 691 |
// move 1cm forward |
... | ... | |
716 | 720 |
global.robot.requestCharging(0); |
717 | 721 |
}else{ |
718 | 722 |
global.motorcontrol.setMotorEnable(true); |
719 |
// TODO: Use controlled
|
|
723 |
// TODO: Use controlled |
|
720 | 724 |
//Rotate -20° to free from magnet |
721 | 725 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
722 | 726 |
checkForMotion(); |
... | ... | |
805 | 809 |
{global.stateTracker[states::CALIBRATION_CHECK] += 1;} |
806 | 810 |
else if (newState == states::DEVIATION_CORRECTION) |
807 | 811 |
{global.stateTracker[states::DEVIATION_CORRECTION] += 1;} |
808 |
|
|
812 |
|
|
809 | 813 |
else if (newState == states::DOCKING_ERROR){global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;} |
810 | 814 |
else if (newState == states::REVERSE_TIMEOUT_ERROR){global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;} |
811 | 815 |
else if (newState == states::CALIBRATION_ERROR){global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;} |
... | ... | |
821 | 825 |
// // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
822 | 826 |
// global.robot.transmitState(currentState); |
823 | 827 |
// // global.robot.setOdometry(global.odometry.getPosition()); |
824 |
|
|
828 |
|
|
825 | 829 |
// }else{ |
826 | 830 |
// utCount.stateCount++; |
827 | 831 |
// } |
Also available in: Unified diff