amiro-os / devices / DiWheelDrive / userthread.cpp @ 84b4c632
History | View | Annotate | Download (24.1 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) > 200 /* || (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 |
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 |
|
250 |
|
251 |
UserThread::UserThread() : |
252 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
253 |
{ |
254 |
} |
255 |
|
256 |
UserThread::~UserThread() |
257 |
{ |
258 |
} |
259 |
|
260 |
msg_t |
261 |
UserThread::main() |
262 |
{ |
263 |
/*
|
264 |
* SETUP
|
265 |
*/
|
266 |
// User thread state:
|
267 |
|
268 |
for (uint8_t led = 0; led < 8; ++led) { |
269 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
270 |
} |
271 |
running = false;
|
272 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
273 |
LineFollow lf(&global); |
274 |
/*
|
275 |
* LOOP
|
276 |
*/
|
277 |
while (!this->shouldTerminate()) |
278 |
{ |
279 |
/*
|
280 |
* read accelerometer z-value
|
281 |
*/
|
282 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
283 |
|
284 |
if (accel_z < -900 /*-0.9g*/) { |
285 |
// Start line following when AMiRo is rotated
|
286 |
if(currentState == states::INACTIVE){
|
287 |
newState = states::FOLLOW_LINE; |
288 |
}else{
|
289 |
newState = states::IDLE; |
290 |
} |
291 |
lightAllLeds(Color::GREEN); |
292 |
this->sleep(1000); |
293 |
lightAllLeds(Color::BLACK); |
294 |
|
295 |
// If message was received handle it here:
|
296 |
} else if(global.msgReceived){ |
297 |
global.msgReceived = false;
|
298 |
// running = true;
|
299 |
switch(global.lfStrategy){
|
300 |
case msg_content::MSG_START:
|
301 |
newState = states::CALIBRATION_CHECK; |
302 |
break;
|
303 |
case msg_content::MSG_STOP:
|
304 |
newState = states::IDLE; |
305 |
break;
|
306 |
case msg_content::MSG_EDGE_RIGHT:
|
307 |
// newState = states::FOLLOW_LINE;
|
308 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
309 |
break;
|
310 |
case msg_content::MSG_EDGE_LEFT:
|
311 |
// newState = states::FOLLOW_LINE;
|
312 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
313 |
break;
|
314 |
case msg_content::MSG_FUZZY:
|
315 |
// newState = states::FOLLOW_LINE;
|
316 |
lStrategy = LineFollowStrategy::FUZZY; |
317 |
break;
|
318 |
case msg_content::MSG_DOCK:
|
319 |
newState = states::DETECT_STATION; |
320 |
break;
|
321 |
case msg_content::MSG_UNDOCK:
|
322 |
newState = states::RELEASE; |
323 |
break;
|
324 |
case msg_content::MSG_CHARGE:
|
325 |
newState = states::CHARGING; |
326 |
break;
|
327 |
case msg_content::MSG_RESET_ODOMETRY:
|
328 |
global.odometry.resetPosition(); |
329 |
break;
|
330 |
case msg_content::MSG_CALIBRATE_BLACK:
|
331 |
proxCalib.calibrateBlack = true;
|
332 |
// global.odometry.resetPosition();
|
333 |
newState = states::CALIBRATION; |
334 |
break;
|
335 |
case msg_content::MSG_CALIBRATE_WHITE:
|
336 |
proxCalib.calibrateBlack = false;
|
337 |
newState = states::CALIBRATION; |
338 |
break;
|
339 |
default:
|
340 |
newState = states::IDLE; |
341 |
break;
|
342 |
} |
343 |
} |
344 |
// newState = currentState;
|
345 |
|
346 |
// Get sensor data
|
347 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
348 |
// uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
349 |
for(int i=0; i<8;i++){ |
350 |
rProx[i] = global.robot.getProximityRingValue(i); |
351 |
} |
352 |
|
353 |
// Continously update devication values
|
354 |
meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0); |
355 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
356 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
357 |
switch(currentState){
|
358 |
case states::INACTIVE:
|
359 |
// Dummy state to deactivate every interaction
|
360 |
break;
|
361 |
// ---------------------------------------
|
362 |
case states::CALIBRATION_CHECK:
|
363 |
// global.robot.calibrate();
|
364 |
if(global.linePID.BThresh >= global.linePID.WThresh){
|
365 |
newState = states::CALIBRATION_ERROR; |
366 |
}else{
|
367 |
newState = states::FOLLOW_LINE; |
368 |
} |
369 |
break;
|
370 |
// ---------------------------------------
|
371 |
case states::CALIBRATION:
|
372 |
/* Calibrate the global thresholds for black or white.
|
373 |
This values will be used by the line follow object
|
374 |
*/
|
375 |
|
376 |
proxCalib.buf = 0;
|
377 |
if(proxCalib.calibrateBlack){
|
378 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
|
379 |
global.robot.calibrate(); |
380 |
} |
381 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
382 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
383 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
384 |
this->sleep(CAN::UPDATE_PERIOD);
|
385 |
} |
386 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
|
387 |
|
388 |
if(proxCalib.calibrateBlack){
|
389 |
global.linePID.BThresh = proxCalib.buf; |
390 |
}else {
|
391 |
global.linePID.WThresh = proxCalib.buf; |
392 |
} |
393 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
|
394 |
|
395 |
newState = states::IDLE; |
396 |
break;
|
397 |
// ---------------------------------------
|
398 |
case states::IDLE:
|
399 |
global.motorcontrol.setMotorEnable(true);
|
400 |
setRpmSpeed(stop); |
401 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
402 |
global.robot.requestCharging(0);
|
403 |
} |
404 |
// pCtrl.pFactor = 0;
|
405 |
pCtrl.staticCont = 0;
|
406 |
utCount.whiteCount = 0;
|
407 |
utCount.ringProxCount = 0;
|
408 |
utCount.errorCount = 0;
|
409 |
newState = states::INACTIVE; |
410 |
break;
|
411 |
// ---------------------------------------
|
412 |
case states::FOLLOW_LINE:
|
413 |
// Set correct forward speed to every strategy
|
414 |
if (global.forwardSpeed != global.rpmForward[0]){ |
415 |
global.forwardSpeed = global.rpmForward[0];
|
416 |
} |
417 |
|
418 |
if(lf.getStrategy() != lStrategy){
|
419 |
lf.setStrategy(lStrategy); |
420 |
} |
421 |
|
422 |
if(lf.followLine(rpmSpeed)){
|
423 |
utCount.whiteCount++; |
424 |
if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
425 |
setRpmSpeed(stop); |
426 |
utCount.whiteCount = 0;
|
427 |
newState = states::WHITE_DETECTION_ERROR; |
428 |
} |
429 |
}else{
|
430 |
utCount.whiteCount = 0;
|
431 |
} |
432 |
|
433 |
preventCollision(rpmSpeed, rProx); |
434 |
// proxSectorSpeedCorrection(rpmSpeed, rProx);
|
435 |
|
436 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
437 |
utCount.ringProxCount = 0;
|
438 |
newState = states::TURN; |
439 |
} |
440 |
|
441 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
442 |
setRpmSpeedFuzzy(rpmSpeed); |
443 |
}else{
|
444 |
|
445 |
setRpmSpeed(rpmSpeed); |
446 |
} |
447 |
|
448 |
break;
|
449 |
// ---------------------------------------
|
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(); |
457 |
newState = states::FOLLOW_LINE; |
458 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
459 |
}else{
|
460 |
newState = states::PROXY_DETECTION_ERROR; |
461 |
} |
462 |
break;
|
463 |
// ---------------------------------------
|
464 |
case states::DETECT_STATION:
|
465 |
if (global.forwardSpeed != DETECTION_SPEED){
|
466 |
global.forwardSpeed = DETECTION_SPEED; |
467 |
} |
468 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
469 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
470 |
} |
471 |
|
472 |
lf.followLine(rpmSpeed); |
473 |
setRpmSpeed(rpmSpeed); |
474 |
// // Detect marker before docking station
|
475 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){
|
476 |
// Use proxy ring
|
477 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
478 |
|
479 |
setRpmSpeed(stop); |
480 |
checkForMotion(); |
481 |
// 180° Rotation
|
482 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
483 |
// BaseThread::sleep(8000);
|
484 |
checkForMotion(); |
485 |
newState = states::CORRECT_POSITIONING; |
486 |
} |
487 |
break;
|
488 |
// ---------------------------------------
|
489 |
case states::CORRECT_POSITIONING:
|
490 |
if (global.forwardSpeed != CHARGING_SPEED){
|
491 |
global.forwardSpeed = CHARGING_SPEED; |
492 |
} |
493 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
494 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
495 |
} |
496 |
lf.followLine(rpmSpeed); |
497 |
setRpmSpeed(rpmSpeed); |
498 |
|
499 |
utCount.stateTime++; |
500 |
if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
501 |
utCount.stateTime = 0;
|
502 |
newState = states::REVERSE; |
503 |
setRpmSpeed(stop); |
504 |
checkForMotion(); |
505 |
} |
506 |
break;
|
507 |
// ---------------------------------------
|
508 |
case states::REVERSE:
|
509 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
510 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
511 |
} |
512 |
lf.followLine(rpmSpeed); |
513 |
setRpmSpeed(rpmSpeed); |
514 |
// utCount.stateTime++;
|
515 |
|
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) )){ |
518 |
// setRpmSpeed(stop);
|
519 |
// checkForMotion();
|
520 |
utCount.stateTime = 0;
|
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; |
536 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
537 |
setRpmSpeed(stop); |
538 |
utCount.stateTime = 0;
|
539 |
utCount.errorCount++; |
540 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
541 |
newState = states::DOCKING_ERROR; |
542 |
}else{
|
543 |
newState = states::CORRECT_POSITIONING; |
544 |
} |
545 |
} |
546 |
|
547 |
// if((devCor.currentDeviation <= -10)){
|
548 |
// rpmSpeed[0] -= 2000000;
|
549 |
// }else if(devCor.currentDeviation >= 10){
|
550 |
// rpmSpeed[1] -= 2000000;
|
551 |
// }
|
552 |
// setRpmSpeed(rpmSpeed);
|
553 |
break;
|
554 |
// ---------------------------------------
|
555 |
case states::DEVIATION_CORRECTION:
|
556 |
// if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
557 |
// lf.setStrategy(LineFollowStrategy::REVERSE);
|
558 |
// }
|
559 |
// lf.followLine(rpmSpeed);
|
560 |
// setRpmSpeed(rpmSpeed);
|
561 |
if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){ |
562 |
if(devCor.RCase){
|
563 |
rpmSpeed[0] = 0; |
564 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
565 |
}else {
|
566 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
567 |
rpmSpeed[1] = 0; |
568 |
} |
569 |
setRpmSpeed(rpmSpeed); |
570 |
}else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){ |
571 |
if(devCor.RCase){
|
572 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
573 |
rpmSpeed[1] = 0; |
574 |
}else {
|
575 |
rpmSpeed[0] = 0; |
576 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
577 |
} |
578 |
setRpmSpeed(rpmSpeed); |
579 |
if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){ |
580 |
utCount.stateTime = 0;
|
581 |
newState = states::REVERSE; |
582 |
setRpmSpeed(stop); |
583 |
} |
584 |
}else{
|
585 |
utCount.stateTime = 0;
|
586 |
newState = states::REVERSE; |
587 |
setRpmSpeed(stop); |
588 |
} |
589 |
|
590 |
utCount.stateTime++; |
591 |
|
592 |
|
593 |
// if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
594 |
// utCount.stateTime = 0;
|
595 |
// newState = states::CHECK_POSITIONING;
|
596 |
// }
|
597 |
break;
|
598 |
// ---------------------------------------
|
599 |
case states::PUSH_BACK:
|
600 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
601 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
602 |
} |
603 |
lf.followLine(rpmSpeed); |
604 |
setRpmSpeed(rpmSpeed); |
605 |
|
606 |
utCount.stateTime++; |
607 |
if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
608 |
utCount.stateTime = 0;
|
609 |
newState = states::CHECK_POSITIONING; |
610 |
} |
611 |
break;
|
612 |
// ---------------------------------------
|
613 |
case states::CHECK_POSITIONING:
|
614 |
setRpmSpeed(stop); |
615 |
checkForMotion(); |
616 |
if(checkDockingSuccess()){
|
617 |
newState = states::CHECK_VOLTAGE; |
618 |
}else{
|
619 |
utCount.errorCount++; |
620 |
newState = states::CORRECT_POSITIONING; |
621 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
622 |
newState = states::DOCKING_ERROR; |
623 |
} |
624 |
} |
625 |
break;
|
626 |
// ---------------------------------------
|
627 |
case states::CHECK_VOLTAGE:
|
628 |
if(!checkPinEnabled()){
|
629 |
global.robot.requestCharging(1);
|
630 |
} else {
|
631 |
if(checkPinVoltage()){
|
632 |
// Pins are under voltage -> correctly docked
|
633 |
|
634 |
newState = states::CHARGING; |
635 |
}else{
|
636 |
utCount.errorCount++; |
637 |
// No voltage on pins -> falsely docked
|
638 |
// deactivate pins
|
639 |
global.motorcontrol.setMotorEnable(true);
|
640 |
global.robot.requestCharging(0);
|
641 |
// TODO: Soft release when docking falsely
|
642 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
643 |
newState = states::RELEASE_TO_CORRECT; |
644 |
} else {
|
645 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
|
646 |
} |
647 |
|
648 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){
|
649 |
newState = states::DOCKING_ERROR; |
650 |
} |
651 |
} |
652 |
} |
653 |
break;
|
654 |
// ---------------------------------------
|
655 |
case states::RELEASE_TO_CORRECT:
|
656 |
|
657 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
658 |
checkForMotion(); |
659 |
// move 1cm forward
|
660 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
661 |
checkForMotion(); |
662 |
// rotate back
|
663 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
664 |
checkForMotion(); |
665 |
|
666 |
global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION); |
667 |
checkForMotion(); |
668 |
newState = states::CORRECT_POSITIONING; |
669 |
break;
|
670 |
// ---------------------------------------
|
671 |
case states::CHARGING:
|
672 |
global.motorcontrol.setMotorEnable(false);
|
673 |
utCount.errorCount = 0;
|
674 |
// Formulate Request to enable charging
|
675 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
676 |
global.robot.requestCharging(1);
|
677 |
} |
678 |
if(checkPinEnabled()){
|
679 |
showChargingState(); |
680 |
} |
681 |
break;
|
682 |
// ---------------------------------------
|
683 |
case states::RELEASE:
|
684 |
if (global.forwardSpeed != DETECTION_SPEED){
|
685 |
global.rpmForward[0] = DETECTION_SPEED;
|
686 |
} |
687 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
688 |
global.robot.requestCharging(0);
|
689 |
}else{
|
690 |
global.motorcontrol.setMotorEnable(true);
|
691 |
|
692 |
//Rotate -20° to free from magnet
|
693 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
694 |
checkForMotion(); |
695 |
// move 1cm forward
|
696 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
697 |
checkForMotion(); |
698 |
// rotate back
|
699 |
// global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
700 |
// checkForMotion();
|
701 |
|
702 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
703 |
// checkForMotion();
|
704 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
705 |
newState = states::FOLLOW_LINE; |
706 |
// whiteBuf = -100;
|
707 |
// lf.followLine(rpmSpeed);
|
708 |
// setRpmSpeed(rpmSpeed);
|
709 |
} |
710 |
// lightAllLeds(Color::BLACK);
|
711 |
break;
|
712 |
// ---------------------------------------
|
713 |
case states::DOCKING_ERROR:
|
714 |
newState = states::RELEASE; |
715 |
break;
|
716 |
// ---------------------------------------
|
717 |
case states::REVERSE_TIMEOUT_ERROR:
|
718 |
newState = states::IDLE; |
719 |
break;
|
720 |
// ---------------------------------------
|
721 |
case states::CALIBRATION_ERROR:
|
722 |
newState = states::IDLE; |
723 |
break;
|
724 |
// ---------------------------------------
|
725 |
case states::WHITE_DETECTION_ERROR:
|
726 |
newState = states::IDLE; |
727 |
break;
|
728 |
// ---------------------------------------
|
729 |
case states::PROXY_DETECTION_ERROR:
|
730 |
newState = states::IDLE; |
731 |
break;
|
732 |
// ---------------------------------------
|
733 |
case states::NO_CHARGING_POWER_ERROR:
|
734 |
newState = states::IDLE; |
735 |
break;
|
736 |
// ---------------------------------------
|
737 |
case states::UNKNOWN_STATE_ERROR:
|
738 |
newState = states::IDLE; |
739 |
break;
|
740 |
// ---------------------------------------
|
741 |
default:
|
742 |
newState = states::UNKNOWN_STATE_ERROR; |
743 |
break;
|
744 |
} |
745 |
if (currentState != newState){
|
746 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
747 |
global.robot.transmitState(newState); |
748 |
} |
749 |
prevState = currentState; |
750 |
currentState = newState; |
751 |
// if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
752 |
// utCount.stateCount = 0;
|
753 |
// // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
754 |
// global.robot.transmitState(currentState);
|
755 |
// // global.robot.setOdometry(global.odometry.getPosition());
|
756 |
|
757 |
// }else{
|
758 |
// utCount.stateCount++;
|
759 |
// }
|
760 |
this->sleep(CAN::UPDATE_PERIOD);
|
761 |
} |
762 |
|
763 |
return RDY_OK;
|
764 |
} |