amiro-os / devices / DiWheelDrive / userthread.cpp @ 10bf9cc0
History | View | Annotate | Download (20.914 KB)
1 |
// #include "userthread.hpp"
|
---|---|
2 |
#include "global.hpp" |
3 |
#include <cmath> |
4 |
#include "linefollow.hpp" |
5 |
// #include <cmath>
|
6 |
// #include "global.hpp"
|
7 |
using namespace amiro; |
8 |
|
9 |
extern Global global;
|
10 |
|
11 |
// a buffer for the z-value of the accelerometer
|
12 |
int16_t accel_z; |
13 |
bool running = false; |
14 |
|
15 |
|
16 |
/**
|
17 |
* Set speed.
|
18 |
*
|
19 |
* @param rpmSpeed speed for left and right wheel in rounds/min
|
20 |
*/
|
21 |
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) { |
22 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
23 |
} |
24 |
|
25 |
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) { |
26 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
27 |
} |
28 |
|
29 |
void UserThread::lightOneLed(Color color, int idx){ |
30 |
global.robot.setLightColor(idx, Color(color)); |
31 |
} |
32 |
|
33 |
void UserThread::lightAllLeds(Color color){
|
34 |
int led = 0; |
35 |
for(led=0; led<8; led++){ |
36 |
lightOneLed(color, led); |
37 |
} |
38 |
} |
39 |
|
40 |
void UserThread::showChargingState(){
|
41 |
uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
42 |
Color color = Color::GREEN; |
43 |
if (numLeds <= 2){ |
44 |
color = Color::RED; |
45 |
}else if(numLeds <= 6){ |
46 |
color = Color::YELLOW; |
47 |
} |
48 |
for (int i=0; i<numLeds; i++){ |
49 |
lightOneLed(color, i); |
50 |
this->sleep(300); |
51 |
} |
52 |
this->sleep(1000); |
53 |
lightAllLeds(Color::BLACK); |
54 |
} |
55 |
|
56 |
void UserThread::chargeAsLED(){
|
57 |
uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
58 |
Color color = Color::GREEN; |
59 |
if (numLeds <= 2){ |
60 |
color = Color::RED; |
61 |
}else if(numLeds <= 6){ |
62 |
color = Color::YELLOW; |
63 |
} |
64 |
for (int i=0; i<numLeds; i++){ |
65 |
lightOneLed(color, i); |
66 |
// this->sleep(300);
|
67 |
} |
68 |
// this->sleep(1000);
|
69 |
// lightAllLeds(Color::BLACK);
|
70 |
} |
71 |
|
72 |
// ----------------------------------------------------------------
|
73 |
|
74 |
void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){ |
75 |
for (int i=0; i<8; i++){ |
76 |
sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8]; |
77 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
|
78 |
|
79 |
} |
80 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
|
81 |
|
82 |
} |
83 |
|
84 |
|
85 |
void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){ |
86 |
for (int i=2; i<5; i++){ |
87 |
sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax; |
88 |
} |
89 |
} |
90 |
|
91 |
void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){ |
92 |
int i;
|
93 |
uint16_t sProx[8];
|
94 |
int32_t sPMax = 0;
|
95 |
getProxySectorVals(proxVals, sProx); |
96 |
getMaxFrontSectorVal(sProx, sPMax); |
97 |
|
98 |
int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
|
99 |
int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
|
100 |
|
101 |
|
102 |
|
103 |
if(sPMax > pCtrl.threshMid){
|
104 |
rpmSpeed[0] = 0; |
105 |
rpmSpeed[1] = 0; |
106 |
pCtrl.staticCont++; |
107 |
}else if((speedL > 0) || (speedR > 0)){ |
108 |
pCtrl.staticCont = 0;
|
109 |
rpmSpeed[0] = speedL;
|
110 |
rpmSpeed[1] = speedR;
|
111 |
}else{
|
112 |
rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000); |
113 |
rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000); |
114 |
} |
115 |
|
116 |
for(i=4; i<5; i++){ |
117 |
if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){ |
118 |
rpmSpeed[0] = -5000000 ; |
119 |
rpmSpeed[1] = -5000000 ; |
120 |
// pCtrl.staticCont++;
|
121 |
break;
|
122 |
} |
123 |
} |
124 |
chargeAsLED(); |
125 |
|
126 |
// 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]);
|
127 |
|
128 |
|
129 |
} |
130 |
// -------------------------------------------------------------------
|
131 |
|
132 |
|
133 |
void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) { |
134 |
|
135 |
if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){ |
136 |
rpmSpeed[0] = rpmSpeed[0] / 2; |
137 |
rpmSpeed[1] = rpmSpeed[1] / 2; |
138 |
} |
139 |
|
140 |
if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){ |
141 |
rpmSpeed[0] = rpmSpeed[0] / 4; |
142 |
rpmSpeed[1] = rpmSpeed[1] / 4; |
143 |
} |
144 |
|
145 |
if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){ |
146 |
rpmSpeed[0] = 0; |
147 |
rpmSpeed[1] = 0; |
148 |
utCount.ringProxCount++; |
149 |
}else{
|
150 |
utCount.ringProxCount = 0;
|
151 |
} |
152 |
|
153 |
} |
154 |
|
155 |
|
156 |
/**
|
157 |
* Blocks as long as the position changes.
|
158 |
*/
|
159 |
void UserThread::checkForMotion(){
|
160 |
int motion = 1; |
161 |
int led = 0; |
162 |
types::position oldPos = global.odometry.getPosition(); |
163 |
while(motion){
|
164 |
this->sleep(500); |
165 |
types::position tmp = global.odometry.getPosition(); |
166 |
motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
167 |
oldPos = tmp; |
168 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
169 |
global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
170 |
led++; |
171 |
} |
172 |
lightAllLeds(Color::BLACK); |
173 |
} |
174 |
|
175 |
bool UserThread::checkFrontalObject(){
|
176 |
uint32_t thresh = pCtrl.threshMid; |
177 |
uint32_t prox; |
178 |
for(int i=0; i<8; i++){ |
179 |
prox = global.robot.getProximityRingValue(i); |
180 |
if((i == 3) || (i == 4)){ |
181 |
if(prox < thresh){
|
182 |
return false; |
183 |
} |
184 |
}else{
|
185 |
if(prox > thresh){
|
186 |
return false; |
187 |
} |
188 |
} |
189 |
} |
190 |
return true; |
191 |
} |
192 |
|
193 |
bool UserThread::checkPinVoltage(){
|
194 |
return global.ltc4412.isPluggedIn();
|
195 |
} |
196 |
|
197 |
bool UserThread::checkPinEnabled(){
|
198 |
return global.ltc4412.isEnabled();
|
199 |
} |
200 |
|
201 |
int UserThread::checkDockingSuccess(){
|
202 |
// setRpmSpeed(stop);
|
203 |
checkForMotion(); |
204 |
int success = 0; |
205 |
// global.odometry.resetPosition();
|
206 |
types::position start = global.startPos = global.odometry.getPosition(); |
207 |
global.motorcontrol.setMotorEnable(false);
|
208 |
this->sleep(1000); |
209 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
210 |
|
211 |
// Amiro moved, docking was not successful
|
212 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){
|
213 |
if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){ |
214 |
lightAllLeds(Color::RED); |
215 |
// Enable Motor again if docking was not successful
|
216 |
global.motorcontrol.setMotorEnable(true);
|
217 |
success = 0;
|
218 |
}else{
|
219 |
lightAllLeds(Color::GREEN); |
220 |
success = 1;
|
221 |
} |
222 |
|
223 |
// this->sleep(500);
|
224 |
lightAllLeds(Color::BLACK); |
225 |
return success;
|
226 |
} |
227 |
|
228 |
int UserThread::getProxyRingSum(){
|
229 |
int prox_sum = 0; |
230 |
for(int i=0; i<8;i++){ |
231 |
prox_sum += global.robot.getProximityRingValue(i);; |
232 |
} |
233 |
return prox_sum;
|
234 |
} |
235 |
|
236 |
|
237 |
UserThread::UserThread() : |
238 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
239 |
{ |
240 |
} |
241 |
|
242 |
UserThread::~UserThread() |
243 |
{ |
244 |
} |
245 |
|
246 |
msg_t |
247 |
UserThread::main() |
248 |
{ |
249 |
/*
|
250 |
* SETUP
|
251 |
*/
|
252 |
// User thread state:
|
253 |
|
254 |
for (uint8_t led = 0; led < 8; ++led) { |
255 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
256 |
} |
257 |
running = false;
|
258 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
259 |
LineFollow lf(&global); |
260 |
/*
|
261 |
* LOOP
|
262 |
*/
|
263 |
while (!this->shouldTerminate()) |
264 |
{ |
265 |
/*
|
266 |
* read accelerometer z-value
|
267 |
*/
|
268 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
269 |
|
270 |
if (accel_z < -900 /*-0.9g*/) { |
271 |
// Start line following when AMiRo is rotated
|
272 |
if(currentState == states::INACTIVE){
|
273 |
newState = states::FOLLOW_LINE; |
274 |
}else{
|
275 |
newState = states::IDLE; |
276 |
} |
277 |
lightAllLeds(Color::GREEN); |
278 |
this->sleep(1000); |
279 |
lightAllLeds(Color::BLACK); |
280 |
|
281 |
// If message was received handle it here:
|
282 |
} else if(global.msgReceived){ |
283 |
global.msgReceived = false;
|
284 |
// running = true;
|
285 |
switch(global.lfStrategy){
|
286 |
case msg_content::MSG_START:
|
287 |
newState = states::CALIBRATION_CHECK; |
288 |
break;
|
289 |
case msg_content::MSG_STOP:
|
290 |
newState = states::IDLE; |
291 |
break;
|
292 |
case msg_content::MSG_EDGE_RIGHT:
|
293 |
// newState = states::FOLLOW_LINE;
|
294 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
295 |
break;
|
296 |
case msg_content::MSG_EDGE_LEFT:
|
297 |
// newState = states::FOLLOW_LINE;
|
298 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
299 |
break;
|
300 |
case msg_content::MSG_FUZZY:
|
301 |
// newState = states::FOLLOW_LINE;
|
302 |
lStrategy = LineFollowStrategy::FUZZY; |
303 |
break;
|
304 |
case msg_content::MSG_DOCK:
|
305 |
newState = states::DETECT_STATION; |
306 |
break;
|
307 |
case msg_content::MSG_UNDOCK:
|
308 |
newState = states::RELEASE; |
309 |
break;
|
310 |
case msg_content::MSG_CHARGE:
|
311 |
newState = states::CHARGING; |
312 |
break;
|
313 |
case msg_content::MSG_RESET_ODOMETRY:
|
314 |
global.odometry.resetPosition(); |
315 |
break;
|
316 |
case msg_content::MSG_CALIBRATE_BLACK:
|
317 |
proxCalib.calibrateBlack = true;
|
318 |
// global.odometry.resetPosition();
|
319 |
newState = states::CALIBRATION; |
320 |
break;
|
321 |
case msg_content::MSG_CALIBRATE_WHITE:
|
322 |
proxCalib.calibrateBlack = false;
|
323 |
newState = states::CALIBRATION; |
324 |
break;
|
325 |
default:
|
326 |
newState = states::IDLE; |
327 |
break;
|
328 |
} |
329 |
} |
330 |
// newState = currentState;
|
331 |
|
332 |
// Get sensor data
|
333 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
334 |
// uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
335 |
for(int i=0; i<8;i++){ |
336 |
rProx[i] = global.robot.getProximityRingValue(i); |
337 |
} |
338 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
339 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
340 |
switch(currentState){
|
341 |
case states::INACTIVE:
|
342 |
// Dummy state to deactivate every interaction
|
343 |
break;
|
344 |
// ---------------------------------------
|
345 |
case states::CALIBRATION_CHECK:
|
346 |
// global.robot.calibrate();
|
347 |
if(global.linePID.BThresh >= global.linePID.WThresh){
|
348 |
newState = states::CALIBRATION_ERROR; |
349 |
}else{
|
350 |
newState = states::FOLLOW_LINE; |
351 |
} |
352 |
break;
|
353 |
// ---------------------------------------
|
354 |
case states::CALIBRATION:
|
355 |
/* Calibrate the global thresholds for black or white.
|
356 |
This values will be used by the line follow object
|
357 |
*/
|
358 |
|
359 |
proxCalib.buf = 0;
|
360 |
if(proxCalib.calibrateBlack){
|
361 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
|
362 |
global.robot.calibrate(); |
363 |
} |
364 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
365 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
366 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
367 |
this->sleep(CAN::UPDATE_PERIOD);
|
368 |
} |
369 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
|
370 |
|
371 |
if(proxCalib.calibrateBlack){
|
372 |
global.linePID.BThresh = proxCalib.buf; |
373 |
}else {
|
374 |
global.linePID.WThresh = proxCalib.buf; |
375 |
} |
376 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
|
377 |
|
378 |
newState = states::IDLE; |
379 |
break;
|
380 |
// ---------------------------------------
|
381 |
case states::IDLE:
|
382 |
global.motorcontrol.setMotorEnable(true);
|
383 |
setRpmSpeed(stop); |
384 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
385 |
global.robot.requestCharging(0);
|
386 |
} |
387 |
// pCtrl.pFactor = 0;
|
388 |
pCtrl.staticCont = 0;
|
389 |
utCount.whiteCount = 0;
|
390 |
utCount.ringProxCount = 0;
|
391 |
utCount.errorCount = 0;
|
392 |
newState = states::INACTIVE; |
393 |
break;
|
394 |
// ---------------------------------------
|
395 |
case states::FOLLOW_LINE:
|
396 |
// Set correct forward speed to every strategy
|
397 |
if (global.forwardSpeed != global.rpmForward[0]){ |
398 |
global.forwardSpeed = global.rpmForward[0];
|
399 |
} |
400 |
|
401 |
if(lf.getStrategy() != lStrategy){
|
402 |
lf.setStrategy(lStrategy); |
403 |
} |
404 |
|
405 |
if(lf.followLine(rpmSpeed)){
|
406 |
utCount.whiteCount++; |
407 |
if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
408 |
setRpmSpeed(stop); |
409 |
utCount.whiteCount = 0;
|
410 |
newState = states::WHITE_DETECTION_ERROR; |
411 |
} |
412 |
}else{
|
413 |
utCount.whiteCount = 0;
|
414 |
} |
415 |
|
416 |
preventCollision(rpmSpeed, rProx); |
417 |
// proxSectorSpeedCorrection(rpmSpeed, rProx);
|
418 |
|
419 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
420 |
utCount.ringProxCount = 0;
|
421 |
newState = states::TURN; |
422 |
} |
423 |
|
424 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
425 |
setRpmSpeedFuzzy(rpmSpeed); |
426 |
}else{
|
427 |
|
428 |
setRpmSpeed(rpmSpeed); |
429 |
} |
430 |
|
431 |
break;
|
432 |
// ---------------------------------------
|
433 |
case states::TURN:
|
434 |
checkForMotion(); |
435 |
// Check if only front sensors are active
|
436 |
if(checkFrontalObject()){
|
437 |
global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION); |
438 |
// BaseThread::sleep(8000);
|
439 |
checkForMotion(); |
440 |
newState = states::FOLLOW_LINE; |
441 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
442 |
}else{
|
443 |
newState = states::PROXY_DETECTION_ERROR; |
444 |
} |
445 |
break;
|
446 |
// ---------------------------------------
|
447 |
case states::DETECT_STATION:
|
448 |
if (global.forwardSpeed != DETECTION_SPEED){
|
449 |
global.forwardSpeed = DETECTION_SPEED; |
450 |
} |
451 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
452 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
453 |
} |
454 |
|
455 |
lf.followLine(rpmSpeed); |
456 |
setRpmSpeed(rpmSpeed); |
457 |
// // Detect marker before docking station
|
458 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){
|
459 |
// Use proxy ring
|
460 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
461 |
|
462 |
setRpmSpeed(stop); |
463 |
checkForMotion(); |
464 |
// 180° Rotation
|
465 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
466 |
// BaseThread::sleep(8000);
|
467 |
checkForMotion(); |
468 |
newState = states::CORRECT_POSITIONING; |
469 |
} |
470 |
break;
|
471 |
// ---------------------------------------
|
472 |
case states::CORRECT_POSITIONING:
|
473 |
if (global.forwardSpeed != CHARGING_SPEED){
|
474 |
global.forwardSpeed = CHARGING_SPEED; |
475 |
} |
476 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
477 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
478 |
} |
479 |
lf.followLine(rpmSpeed); |
480 |
setRpmSpeed(rpmSpeed); |
481 |
|
482 |
utCount.stateTime++; |
483 |
if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
484 |
utCount.stateTime = 0;
|
485 |
newState = states::REVERSE; |
486 |
setRpmSpeed(stop); |
487 |
checkForMotion(); |
488 |
} |
489 |
break;
|
490 |
// ---------------------------------------
|
491 |
case states::REVERSE:
|
492 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
493 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
494 |
} |
495 |
lf.followLine(rpmSpeed); |
496 |
setRpmSpeed(rpmSpeed); |
497 |
utCount.stateTime++; |
498 |
|
499 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
500 |
// setRpmSpeed(stop);
|
501 |
// checkForMotion();
|
502 |
utCount.stateTime = 0;
|
503 |
newState = states::PUSH_BACK; |
504 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
505 |
setRpmSpeed(stop); |
506 |
utCount.stateTime = 0;
|
507 |
utCount.errorCount++; |
508 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
509 |
newState = states::DOCKING_ERROR; |
510 |
}else{
|
511 |
newState = states::CORRECT_POSITIONING; |
512 |
} |
513 |
} |
514 |
|
515 |
break;
|
516 |
// ---------------------------------------
|
517 |
case states::PUSH_BACK:
|
518 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
519 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
520 |
} |
521 |
lf.followLine(rpmSpeed); |
522 |
setRpmSpeed(rpmSpeed); |
523 |
|
524 |
utCount.stateTime++; |
525 |
if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
526 |
utCount.stateTime = 0;
|
527 |
newState = states::CHECK_POSITIONING; |
528 |
} |
529 |
break;
|
530 |
// ---------------------------------------
|
531 |
case states::CHECK_POSITIONING:
|
532 |
setRpmSpeed(stop); |
533 |
checkForMotion(); |
534 |
if(checkDockingSuccess()){
|
535 |
newState = states::CHECK_VOLTAGE; |
536 |
}else{
|
537 |
utCount.errorCount++; |
538 |
newState = states::CORRECT_POSITIONING; |
539 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
540 |
newState = states::DOCKING_ERROR; |
541 |
} |
542 |
} |
543 |
break;
|
544 |
// ---------------------------------------
|
545 |
case states::CHECK_VOLTAGE:
|
546 |
if(!checkPinEnabled()){
|
547 |
global.robot.requestCharging(1);
|
548 |
} else {
|
549 |
if(checkPinVoltage()){
|
550 |
// Pins are under voltage -> correctly docked
|
551 |
|
552 |
newState = states::CHARGING; |
553 |
}else{
|
554 |
utCount.errorCount++; |
555 |
// No voltage on pins -> falsely docked
|
556 |
// deactivate pins
|
557 |
global.motorcontrol.setMotorEnable(true);
|
558 |
global.robot.requestCharging(0);
|
559 |
// TODO: Soft release when docking falsely
|
560 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
561 |
newState = states::RELEASE_TO_CORRECT; |
562 |
} else {
|
563 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
|
564 |
} |
565 |
|
566 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){
|
567 |
newState = states::DOCKING_ERROR; |
568 |
} |
569 |
} |
570 |
} |
571 |
break;
|
572 |
// ---------------------------------------
|
573 |
case states::RELEASE_TO_CORRECT:
|
574 |
|
575 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
576 |
checkForMotion(); |
577 |
// move 1cm forward
|
578 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
579 |
checkForMotion(); |
580 |
// rotate back
|
581 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
582 |
checkForMotion(); |
583 |
|
584 |
global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION); |
585 |
checkForMotion(); |
586 |
newState = states::CORRECT_POSITIONING; |
587 |
break;
|
588 |
// ---------------------------------------
|
589 |
case states::CHARGING:
|
590 |
global.motorcontrol.setMotorEnable(false);
|
591 |
utCount.errorCount = 0;
|
592 |
// Formulate Request to enable charging
|
593 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
594 |
global.robot.requestCharging(1);
|
595 |
} |
596 |
if(checkPinEnabled()){
|
597 |
showChargingState(); |
598 |
} |
599 |
break;
|
600 |
// ---------------------------------------
|
601 |
case states::RELEASE:
|
602 |
if (global.forwardSpeed != DETECTION_SPEED){
|
603 |
global.rpmForward[0] = DETECTION_SPEED;
|
604 |
} |
605 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
606 |
global.robot.requestCharging(0);
|
607 |
}else{
|
608 |
global.motorcontrol.setMotorEnable(true);
|
609 |
|
610 |
//Rotate -20° to free from magnet
|
611 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
612 |
checkForMotion(); |
613 |
// move 1cm forward
|
614 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
615 |
checkForMotion(); |
616 |
// rotate back
|
617 |
// global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
618 |
// checkForMotion();
|
619 |
|
620 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
621 |
// checkForMotion();
|
622 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
623 |
newState = states::FOLLOW_LINE; |
624 |
// whiteBuf = -100;
|
625 |
// lf.followLine(rpmSpeed);
|
626 |
// setRpmSpeed(rpmSpeed);
|
627 |
} |
628 |
// lightAllLeds(Color::BLACK);
|
629 |
break;
|
630 |
// ---------------------------------------
|
631 |
case states::DOCKING_ERROR:
|
632 |
newState = states::RELEASE; |
633 |
break;
|
634 |
// ---------------------------------------
|
635 |
case states::REVERSE_TIMEOUT_ERROR:
|
636 |
newState = states::IDLE; |
637 |
break;
|
638 |
// ---------------------------------------
|
639 |
case states::CALIBRATION_ERROR:
|
640 |
newState = states::IDLE; |
641 |
break;
|
642 |
// ---------------------------------------
|
643 |
case states::WHITE_DETECTION_ERROR:
|
644 |
newState = states::IDLE; |
645 |
break;
|
646 |
// ---------------------------------------
|
647 |
case states::PROXY_DETECTION_ERROR:
|
648 |
newState = states::IDLE; |
649 |
break;
|
650 |
// ---------------------------------------
|
651 |
case states::NO_CHARGING_POWER_ERROR:
|
652 |
newState = states::IDLE; |
653 |
break;
|
654 |
// ---------------------------------------
|
655 |
case states::UNKNOWN_STATE_ERROR:
|
656 |
newState = states::IDLE; |
657 |
break;
|
658 |
// ---------------------------------------
|
659 |
default:
|
660 |
newState = states::UNKNOWN_STATE_ERROR; |
661 |
break;
|
662 |
} |
663 |
if (currentState != newState){
|
664 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
665 |
global.robot.transmitState(newState); |
666 |
} |
667 |
prevState = currentState; |
668 |
currentState = newState; |
669 |
if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
670 |
utCount.stateCount = 0;
|
671 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
672 |
global.robot.transmitState(currentState); |
673 |
// global.robot.setOdometry(global.odometry.getPosition());
|
674 |
|
675 |
}else{
|
676 |
utCount.stateCount++; |
677 |
} |
678 |
this->sleep(CAN::UPDATE_PERIOD);
|
679 |
} |
680 |
|
681 |
return RDY_OK;
|
682 |
} |