amiro-os / devices / DiWheelDrive / userthread.cpp @ bdac5bec
History | View | Annotate | Download (32.2 KB)
1 |
#include "global.hpp" |
---|---|
2 |
#include <cmath> |
3 |
#include "linefollow.hpp" |
4 |
#include "userthread.hpp" |
5 |
#include "amiro_map.hpp" |
6 |
|
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] / 3; |
142 |
rpmSpeed[1] = rpmSpeed[1] / 3; |
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 |
bool motion = true; |
161 |
int led = 0; |
162 |
types::position oldPos = global.odometry.getPosition(); |
163 |
while(motion){
|
164 |
this->sleep(200); |
165 |
types::position tmp = global.odometry.getPosition(); |
166 |
motion = oldPos.x != tmp.x; //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 |
void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],
|
251 |
uint8_t id, uint8_t l, uint8_t r, uint8_t att) { |
252 |
map[id][0] = l;
|
253 |
map[id][1] = r;
|
254 |
map[id][2] = att;
|
255 |
} |
256 |
|
257 |
UserThread::UserThread() : |
258 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
259 |
{ |
260 |
} |
261 |
|
262 |
UserThread::~UserThread() |
263 |
{ |
264 |
} |
265 |
|
266 |
msg_t |
267 |
UserThread::main() |
268 |
{ |
269 |
/*
|
270 |
* SETUP
|
271 |
*/
|
272 |
// User thread state:
|
273 |
|
274 |
for (uint8_t led = 0; led < 8; ++led) { |
275 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
276 |
} |
277 |
running = false;
|
278 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
279 |
LineFollow lf(&global); |
280 |
AmiroMap map(&global); |
281 |
/*
|
282 |
* LOOP
|
283 |
*/
|
284 |
while (!this->shouldTerminate()) |
285 |
{ |
286 |
/*
|
287 |
* read accelerometer z-value
|
288 |
*/
|
289 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
290 |
|
291 |
if (accel_z < -900 /*-0.9g*/) { |
292 |
// Start line following when AMiRo is rotated
|
293 |
if(currentState == states::INACTIVE){
|
294 |
newState = states::FOLLOW_LINE; |
295 |
}else{
|
296 |
newState = states::IDLE; |
297 |
} |
298 |
lightAllLeds(Color::GREEN); |
299 |
this->sleep(1000); |
300 |
lightAllLeds(Color::BLACK); |
301 |
|
302 |
// If message was received handle it here:
|
303 |
} else if(global.msgReceived){ |
304 |
global.msgReceived = false;
|
305 |
// running = true;
|
306 |
switch(global.lfStrategy){
|
307 |
case msg_content::MSG_START:
|
308 |
newState = states::CALIBRATION_CHECK; |
309 |
break;
|
310 |
case msg_content::MSG_STOP:
|
311 |
newState = states::IDLE; |
312 |
break;
|
313 |
case msg_content::MSG_EDGE_RIGHT:
|
314 |
// newState = states::FOLLOW_LINE;
|
315 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
316 |
break;
|
317 |
case msg_content::MSG_EDGE_LEFT:
|
318 |
// newState = states::FOLLOW_LINE;
|
319 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
320 |
break;
|
321 |
case msg_content::MSG_FUZZY:
|
322 |
// newState = states::FOLLOW_LINE;
|
323 |
lStrategy = LineFollowStrategy::FUZZY; |
324 |
break;
|
325 |
case msg_content::MSG_DOCK:
|
326 |
newState = states::DETECT_STATION; |
327 |
break;
|
328 |
case msg_content::MSG_UNDOCK:
|
329 |
newState = states::RELEASE; |
330 |
break;
|
331 |
case msg_content::MSG_CHARGE:
|
332 |
newState = states::CHARGING; |
333 |
break;
|
334 |
case msg_content::MSG_RESET_ODOMETRY:
|
335 |
global.odometry.resetPosition(); |
336 |
break;
|
337 |
case msg_content::MSG_CALIBRATE_BLACK:
|
338 |
proxCalib.calibrateBlack = true;
|
339 |
// global.odometry.resetPosition();
|
340 |
newState = states::CALIBRATION; |
341 |
break;
|
342 |
case msg_content::MSG_CALIBRATE_WHITE:
|
343 |
proxCalib.calibrateBlack = false;
|
344 |
newState = states::CALIBRATION; |
345 |
break;
|
346 |
case msg_content::MSG_TEST_MAP_STATE:
|
347 |
newState = states::TEST_MAP_STATE; |
348 |
break;
|
349 |
|
350 |
default:
|
351 |
newState = states::IDLE; |
352 |
break;
|
353 |
} |
354 |
} |
355 |
// newState = currentState;
|
356 |
|
357 |
// Get sensor data
|
358 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
359 |
// uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
360 |
for(int i=0; i<8;i++){ |
361 |
rProx[i] = global.robot.getProximityRingValue(i); |
362 |
} |
363 |
|
364 |
// Continously update devication values
|
365 |
meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0); |
366 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
367 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
368 |
switch(currentState){
|
369 |
case states::INACTIVE:
|
370 |
// Dummy state to deactivate every interaction
|
371 |
break;
|
372 |
// ---------------------------------------
|
373 |
case states::CALIBRATION_CHECK:
|
374 |
// global.robot.calibrate();
|
375 |
if(global.linePID.BThresh >= global.linePID.WThresh){
|
376 |
newState = states::CALIBRATION_ERROR; |
377 |
}else{
|
378 |
newState = states::FOLLOW_LINE; |
379 |
} |
380 |
break;
|
381 |
// ---------------------------------------
|
382 |
case states::CALIBRATION:
|
383 |
/* Calibrate the global thresholds for black or white.
|
384 |
This values will be used by the line follow object
|
385 |
*/
|
386 |
|
387 |
proxCalib.buf = 0;
|
388 |
if(proxCalib.calibrateBlack){
|
389 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
|
390 |
global.robot.calibrate(); |
391 |
} |
392 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
393 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
394 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
395 |
this->sleep(CAN::UPDATE_PERIOD);
|
396 |
} |
397 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
|
398 |
|
399 |
if(proxCalib.calibrateBlack){
|
400 |
global.linePID.BThresh = proxCalib.buf; |
401 |
}else {
|
402 |
global.linePID.WThresh = proxCalib.buf; |
403 |
} |
404 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
|
405 |
|
406 |
newState = states::IDLE; |
407 |
break;
|
408 |
// ---------------------------------------
|
409 |
case states::IDLE:
|
410 |
global.motorcontrol.setMotorEnable(true);
|
411 |
setRpmSpeed(stop); |
412 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
413 |
global.robot.requestCharging(0);
|
414 |
} |
415 |
// pCtrl.pFactor = 0;
|
416 |
pCtrl.staticCont = 0;
|
417 |
utCount.whiteCount = 0;
|
418 |
utCount.ringProxCount = 0;
|
419 |
utCount.errorCount = 0;
|
420 |
newState = states::INACTIVE; |
421 |
break;
|
422 |
// ---------------------------------------
|
423 |
case states::FOLLOW_LINE:
|
424 |
// Set correct forward speed to every strategy
|
425 |
if (global.forwardSpeed != global.rpmForward[0]){ |
426 |
global.forwardSpeed = global.rpmForward[0];
|
427 |
} |
428 |
|
429 |
if(lf.getStrategy() != lStrategy){
|
430 |
lf.setStrategy(lStrategy); |
431 |
} |
432 |
|
433 |
if(lf.followLine(rpmSpeed)){
|
434 |
utCount.whiteCount++; |
435 |
if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
436 |
setRpmSpeed(stop); |
437 |
utCount.whiteCount = 0;
|
438 |
newState = states::WHITE_DETECTION_ERROR; |
439 |
} |
440 |
}else{
|
441 |
utCount.whiteCount = 0;
|
442 |
} |
443 |
|
444 |
preventCollision(rpmSpeed, rProx); |
445 |
// proxSectorSpeedCorrection(rpmSpeed, rProx);
|
446 |
|
447 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
448 |
utCount.ringProxCount = 0;
|
449 |
|
450 |
|
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 |
this->utCount.whiteCount = 0; |
458 |
newState = states::TURN; |
459 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
460 |
} else {
|
461 |
newState = states::PROXY_DETECTION_ERROR; |
462 |
} |
463 |
} |
464 |
|
465 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
466 |
setRpmSpeedFuzzy(rpmSpeed); |
467 |
}else{
|
468 |
|
469 |
setRpmSpeed(rpmSpeed); |
470 |
} |
471 |
|
472 |
break;
|
473 |
// ---------------------------------------
|
474 |
case states::TURN:{
|
475 |
// Check the line strategy in order to continue driving on the right side
|
476 |
int factor = SPEED_CONVERSION_FACTOR;
|
477 |
int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
478 |
int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
479 |
int blackSensor = 0; |
480 |
if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
|
481 |
factor = -factor; |
482 |
blackSensor = frontL; |
483 |
}else{
|
484 |
blackSensor = frontR; |
485 |
} |
486 |
|
487 |
rpmSpeed[0] = factor * CHARGING_SPEED;
|
488 |
rpmSpeed[1] = -factor * CHARGING_SPEED;
|
489 |
setRpmSpeed(rpmSpeed); |
490 |
|
491 |
if ((blackSensor >= global.linePID.WThresh )){
|
492 |
utCount.whiteCount = 1;
|
493 |
}else {
|
494 |
if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){ |
495 |
utCount.whiteCount = 0;
|
496 |
newState = states::FOLLOW_LINE; |
497 |
setRpmSpeed(stop); |
498 |
} |
499 |
} |
500 |
break;
|
501 |
} |
502 |
// ---------------------------------------
|
503 |
case states::DETECT_STATION:
|
504 |
|
505 |
if (global.forwardSpeed != DETECTION_SPEED){
|
506 |
global.forwardSpeed = DETECTION_SPEED; |
507 |
} |
508 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
509 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
510 |
} |
511 |
|
512 |
lf.followLine(rpmSpeed); |
513 |
setRpmSpeed(rpmSpeed); |
514 |
// // Detect marker before docking station
|
515 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){
|
516 |
// Use proxy ring
|
517 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
518 |
|
519 |
setRpmSpeed(stop); |
520 |
checkForMotion(); |
521 |
// 180° Rotation
|
522 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
523 |
// BaseThread::sleep(8000);
|
524 |
checkForMotion(); |
525 |
newState = states::CORRECT_POSITIONING; |
526 |
} |
527 |
break;
|
528 |
// ---------------------------------------
|
529 |
case states::CORRECT_POSITIONING:
|
530 |
if (global.forwardSpeed != CHARGING_SPEED){
|
531 |
global.forwardSpeed = CHARGING_SPEED; |
532 |
} |
533 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
534 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
535 |
} |
536 |
lf.followLine(rpmSpeed); |
537 |
setRpmSpeed(rpmSpeed); |
538 |
|
539 |
utCount.stateTime++; |
540 |
if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
541 |
utCount.stateTime = 0;
|
542 |
newState = states::REVERSE; |
543 |
setRpmSpeed(stop); |
544 |
checkForMotion(); |
545 |
} |
546 |
break;
|
547 |
// ---------------------------------------
|
548 |
case states::REVERSE:
|
549 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
550 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
551 |
} |
552 |
lf.followLine(rpmSpeed); |
553 |
setRpmSpeed(rpmSpeed); |
554 |
// utCount.stateTime++;
|
555 |
|
556 |
// Docking is only successful if Deviation is in range and sensors are at their max values.
|
557 |
if((rProx[0] >= PROX_MAX_VAL) |
558 |
&& (rProx[7] >= PROX_MAX_VAL)
|
559 |
&& ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){ |
560 |
// setRpmSpeed(stop);
|
561 |
// checkForMotion();
|
562 |
utCount.stateTime = 0;
|
563 |
newState = states::PUSH_BACK; |
564 |
}else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
565 |
// Case R
|
566 |
utCount.stateTime = 0;
|
567 |
setRpmSpeed(stop); |
568 |
devCor.RCase = true;
|
569 |
lightAllLeds(Color::YELLOW); |
570 |
newState = states::DEVIATION_CORRECTION; |
571 |
}else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
572 |
// Case L
|
573 |
utCount.stateTime = 0;
|
574 |
setRpmSpeed(stop); |
575 |
devCor.RCase = false;
|
576 |
lightAllLeds(Color::WHITE); |
577 |
newState = states::DEVIATION_CORRECTION; |
578 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
579 |
setRpmSpeed(stop); |
580 |
utCount.stateTime = 0;
|
581 |
utCount.errorCount++; |
582 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
583 |
newState = states::DOCKING_ERROR; |
584 |
}else{
|
585 |
newState = states::CORRECT_POSITIONING; |
586 |
} |
587 |
} |
588 |
|
589 |
// if((devCor.currentDeviation <= -10)){
|
590 |
// rpmSpeed[0] -= 2000000;
|
591 |
// }else if(devCor.currentDeviation >= 10){
|
592 |
// rpmSpeed[1] -= 2000000;
|
593 |
// }
|
594 |
// setRpmSpeed(rpmSpeed);
|
595 |
break;
|
596 |
// ---------------------------------------
|
597 |
case states::DEVIATION_CORRECTION:
|
598 |
// if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
599 |
// lf.setStrategy(LineFollowStrategy::REVERSE);
|
600 |
// }
|
601 |
// lf.followLine(rpmSpeed);
|
602 |
// setRpmSpeed(rpmSpeed);
|
603 |
if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){ |
604 |
if(devCor.RCase){
|
605 |
rpmSpeed[0] = 0; |
606 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
607 |
}else {
|
608 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
609 |
rpmSpeed[1] = 0; |
610 |
} |
611 |
setRpmSpeed(rpmSpeed); |
612 |
}else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){ |
613 |
if(devCor.RCase){
|
614 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
615 |
rpmSpeed[1] = 0; |
616 |
}else {
|
617 |
rpmSpeed[0] = 0; |
618 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
619 |
} |
620 |
setRpmSpeed(rpmSpeed); |
621 |
if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){ |
622 |
utCount.stateTime = 0;
|
623 |
newState = states::REVERSE; |
624 |
setRpmSpeed(stop); |
625 |
} |
626 |
}else{
|
627 |
utCount.stateTime = 0;
|
628 |
newState = states::REVERSE; |
629 |
setRpmSpeed(stop); |
630 |
} |
631 |
|
632 |
utCount.stateTime++; |
633 |
|
634 |
|
635 |
// if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
636 |
// utCount.stateTime = 0;
|
637 |
// newState = states::CHECK_POSITIONING;
|
638 |
// }
|
639 |
break;
|
640 |
// ---------------------------------------
|
641 |
case states::PUSH_BACK:
|
642 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
643 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
644 |
} |
645 |
lf.followLine(rpmSpeed); |
646 |
setRpmSpeed(rpmSpeed); |
647 |
|
648 |
utCount.stateTime++; |
649 |
if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
650 |
utCount.stateTime = 0;
|
651 |
newState = states::CHECK_POSITIONING; |
652 |
} |
653 |
break;
|
654 |
// ---------------------------------------
|
655 |
case states::CHECK_POSITIONING:
|
656 |
setRpmSpeed(stop); |
657 |
checkForMotion(); |
658 |
if(checkDockingSuccess()){
|
659 |
newState = states::CHECK_VOLTAGE; |
660 |
}else{
|
661 |
utCount.errorCount++; |
662 |
newState = states::CORRECT_POSITIONING; |
663 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
664 |
newState = states::DOCKING_ERROR; |
665 |
} |
666 |
} |
667 |
break;
|
668 |
// ---------------------------------------
|
669 |
case states::CHECK_VOLTAGE:
|
670 |
if(!checkPinEnabled()){
|
671 |
global.robot.requestCharging(1);
|
672 |
} else {
|
673 |
if(checkPinVoltage()){
|
674 |
// Pins are under voltage -> correctly docked
|
675 |
|
676 |
newState = states::CHARGING; |
677 |
}else{
|
678 |
utCount.errorCount++; |
679 |
// No voltage on pins -> falsely docked
|
680 |
// deactivate pins
|
681 |
global.motorcontrol.setMotorEnable(true);
|
682 |
global.robot.requestCharging(0);
|
683 |
// TODO: Soft release when docking falsely
|
684 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
685 |
newState = states::RELEASE_TO_CORRECT; |
686 |
} else {
|
687 |
newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
|
688 |
} |
689 |
|
690 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){
|
691 |
newState = states::DOCKING_ERROR; |
692 |
} |
693 |
} |
694 |
} |
695 |
break;
|
696 |
// ---------------------------------------
|
697 |
case states::RELEASE_TO_CORRECT:
|
698 |
|
699 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
700 |
checkForMotion(); |
701 |
// move 1cm forward
|
702 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
703 |
checkForMotion(); |
704 |
// rotate back
|
705 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
706 |
checkForMotion(); |
707 |
|
708 |
global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION); |
709 |
checkForMotion(); |
710 |
newState = states::CORRECT_POSITIONING; |
711 |
break;
|
712 |
// ---------------------------------------
|
713 |
case states::CHARGING:
|
714 |
global.motorcontrol.setMotorEnable(false);
|
715 |
utCount.errorCount = 0;
|
716 |
// Formulate Request to enable charging
|
717 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
718 |
global.robot.requestCharging(1);
|
719 |
} |
720 |
if(checkPinEnabled()){
|
721 |
showChargingState(); |
722 |
} |
723 |
break;
|
724 |
// ---------------------------------------
|
725 |
case states::RELEASE:
|
726 |
|
727 |
if (global.forwardSpeed != DETECTION_SPEED){
|
728 |
global.rpmForward[0] = DETECTION_SPEED;
|
729 |
} |
730 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
731 |
global.robot.requestCharging(0);
|
732 |
}else{
|
733 |
global.motorcontrol.setMotorEnable(true);
|
734 |
// TODO: Use controlled
|
735 |
//Rotate -20° to free from magnet
|
736 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
737 |
checkForMotion(); |
738 |
// move 1cm forward
|
739 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
740 |
checkForMotion(); |
741 |
// rotate back
|
742 |
// global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
743 |
// checkForMotion();
|
744 |
|
745 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
746 |
// checkForMotion();
|
747 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
748 |
newState = states::FOLLOW_LINE; |
749 |
// whiteBuf = -100;
|
750 |
// lf.followLine(rpmSpeed);
|
751 |
// setRpmSpeed(rpmSpeed);
|
752 |
} |
753 |
// lightAllLeds(Color::BLACK);
|
754 |
break;
|
755 |
// ---------------------------------------
|
756 |
case states::DOCKING_ERROR:
|
757 |
newState = states::RELEASE; |
758 |
break;
|
759 |
// ---------------------------------------
|
760 |
case states::REVERSE_TIMEOUT_ERROR:
|
761 |
newState = states::IDLE; |
762 |
break;
|
763 |
// ---------------------------------------
|
764 |
case states::CALIBRATION_ERROR:
|
765 |
newState = states::IDLE; |
766 |
break;
|
767 |
// ---------------------------------------
|
768 |
case states::WHITE_DETECTION_ERROR:
|
769 |
newState = states::IDLE; |
770 |
break;
|
771 |
// ---------------------------------------
|
772 |
case states::PROXY_DETECTION_ERROR:
|
773 |
newState = states::IDLE; |
774 |
break;
|
775 |
// ---------------------------------------
|
776 |
case states::NO_CHARGING_POWER_ERROR:
|
777 |
newState = states::IDLE; |
778 |
break;
|
779 |
// ---------------------------------------
|
780 |
case states::UNKNOWN_STATE_ERROR:
|
781 |
newState = states::IDLE; |
782 |
break;
|
783 |
// ---------------------------------------
|
784 |
case states::TEST_MAP_STATE:{
|
785 |
// Test suit for amiro map
|
786 |
|
787 |
|
788 |
|
789 |
// AmiroMap map = AmiroMap(&global);
|
790 |
|
791 |
// --------------------------------------------------
|
792 |
|
793 |
global.tcase = 0;
|
794 |
// Set basic valid map configuration
|
795 |
setAttributes(global.testmap, 0, 1, 2, 1); |
796 |
setAttributes(global.testmap, 1, 2, 2, 0); |
797 |
setAttributes(global.testmap, 2, 1, 0, 0); |
798 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
799 |
chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());
|
800 |
global.testres[global.tcase] = map.get_state()->valid; |
801 |
|
802 |
global.tcase++; // 1
|
803 |
// Test map fail if first node is flagged with end
|
804 |
setAttributes(global.testmap, 0, 1, 2, 0xff); |
805 |
map.initialize(); |
806 |
global.testres[global.tcase] = !map.get_state()->valid; |
807 |
|
808 |
global.tcase++; // 2
|
809 |
// Test if node 2 is set as start node
|
810 |
setAttributes(global.testmap, 0, 1, 2, 0); |
811 |
setAttributes(global.testmap, 2, 1, 0, 1); |
812 |
map.initialize(); |
813 |
global.testres[global.tcase] = map.get_state()->current == 2;
|
814 |
|
815 |
global.tcase++; // 3
|
816 |
// Test if non reachable nodes will trigger invalid map
|
817 |
setAttributes(global.testmap, 3, 0, 0, 0); |
818 |
setAttributes(global.testmap, 4, 0, 0, 0xff); |
819 |
map.initialize(); |
820 |
global.testres[global.tcase] = !map.get_state()->valid; |
821 |
|
822 |
global.tcase++; // 4
|
823 |
// Test Reinitialization
|
824 |
setAttributes(global.testmap, 0, 1, 2, 1); |
825 |
setAttributes(global.testmap, 1, 2, 2, 0); |
826 |
setAttributes(global.testmap, 2, 1, 0, 0); |
827 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
828 |
map.initialize(); |
829 |
global.testres[global.tcase] = map.get_state()->valid; |
830 |
|
831 |
global.odometry.resetPosition(); |
832 |
uint8_t ret = 0;
|
833 |
global.tcase++; // 5
|
834 |
// Test update under normal linefollowing without fixpoint
|
835 |
ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
836 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
837 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
838 |
map.get_state()->current, map.get_state()->next); |
839 |
// No case should be true because neither was a node visited nor
|
840 |
// was a fixpoint detected.
|
841 |
global.testres[global.tcase] = (ret == 0x4);
|
842 |
|
843 |
|
844 |
global.odometry.setPosition(1.0, 0.0, 0.0); |
845 |
global.tcase++; // 6
|
846 |
// Fixpoint on left side
|
847 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
848 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
849 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
850 |
map.get_state()->current, map.get_state()->next); |
851 |
// No case should be true because neither was a node visited nor
|
852 |
// was a fixpoint detected.
|
853 |
// global.odometry
|
854 |
global.testres[global.tcase] = (ret == 0x1)
|
855 |
&& (map.get_state()->strategy == 0x01)
|
856 |
&& (map.get_state()->dist == 0)
|
857 |
&& (map.get_state()->current == 2);
|
858 |
|
859 |
|
860 |
global.odometry.setPosition(1.5, 0.0, 0.0); |
861 |
global.tcase++; // 7
|
862 |
// Fixpoint on left side, no update should appear because fixpoint already
|
863 |
// marked
|
864 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
865 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
866 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
867 |
map.get_state()->current, map.get_state()->next); |
868 |
// No case should be true because neither was a node visited nor
|
869 |
// was a fixpoint detected.
|
870 |
global.testres[global.tcase] = (ret == 0x00)
|
871 |
&& (map.get_state()->strategy == 0x01);
|
872 |
// && (map.get_state()->dist == 0);
|
873 |
|
874 |
|
875 |
|
876 |
|
877 |
|
878 |
int failed = 0; |
879 |
int passed = 0; |
880 |
for (int i = 0; i <= global.tcase; i++) { |
881 |
if (global.testres[i]) {
|
882 |
passed++; |
883 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
884 |
"Test %d Passed!\n", i);
|
885 |
} else {
|
886 |
failed++; |
887 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
888 |
"Test %d Failed\n", i);
|
889 |
} |
890 |
} |
891 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
892 |
"Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed, |
893 |
failed); |
894 |
|
895 |
newState = states::IDLE; |
896 |
break;
|
897 |
} |
898 |
// --------------------------------------------------
|
899 |
default:
|
900 |
newState = states::UNKNOWN_STATE_ERROR; |
901 |
break;
|
902 |
} |
903 |
|
904 |
// In case a new state is set:
|
905 |
// 1. Record the state transition
|
906 |
if (currentState != newState){
|
907 |
|
908 |
global.stateTransitionCounter++; |
909 |
// Clear all state transitions to prevent overflow
|
910 |
if (global.stateTransitionCounter >= 255) { |
911 |
global.stateTransitionCounter = 0;
|
912 |
for (int i = 0; i < 24; i++) { |
913 |
global.stateTracker[i] = 0;
|
914 |
} |
915 |
} |
916 |
// Transmit the new state over can
|
917 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
918 |
global.robot.transmitState(newState); |
919 |
|
920 |
// Increase state count for specific state
|
921 |
// TODO: Improve with dictionary or other than switch case
|
922 |
if (newState == states::IDLE)
|
923 |
{global.stateTracker[states::IDLE] += 1;}
|
924 |
else if (newState == states::FOLLOW_LINE) |
925 |
{global.stateTracker[states::FOLLOW_LINE] += 1;}
|
926 |
else if (newState == states::DETECT_STATION) |
927 |
{global.stateTracker[states::DETECT_STATION] += 1;}
|
928 |
else if (newState == states::REVERSE) |
929 |
{global.stateTracker[states::REVERSE] += 1;}
|
930 |
else if (newState == states::PUSH_BACK) |
931 |
{global.stateTracker[states::PUSH_BACK] += 1;}
|
932 |
else if (newState == states::CHECK_POSITIONING) |
933 |
{global.stateTracker[states::CHECK_POSITIONING] += 1;}
|
934 |
else if (newState == states::CHECK_VOLTAGE) |
935 |
{global.stateTracker[states::CHECK_VOLTAGE] += 1;}
|
936 |
else if (newState == states::CHARGING) |
937 |
{global.stateTracker[states::CHARGING] += 1;}
|
938 |
else if (newState == states::RELEASE) |
939 |
{global.stateTracker[states::RELEASE] += 1;}
|
940 |
else if (newState == states::RELEASE_TO_CORRECT) |
941 |
{global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
|
942 |
else if (newState == states::CORRECT_POSITIONING) |
943 |
{global.stateTracker[states::CORRECT_POSITIONING] += 1;}
|
944 |
else if (newState == states::TURN) |
945 |
{global.stateTracker[states::TURN] += 1;}
|
946 |
else if (newState == states::INACTIVE) |
947 |
{global.stateTracker[states::INACTIVE] += 1;}
|
948 |
else if (newState == states::CALIBRATION) |
949 |
{global.stateTracker[states::CALIBRATION] += 1;}
|
950 |
else if (newState == states::CALIBRATION_CHECK) |
951 |
{global.stateTracker[states::CALIBRATION_CHECK] += 1;}
|
952 |
else if (newState == states::DEVIATION_CORRECTION) |
953 |
{global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
|
954 |
else if (newState == states::DOCKING_ERROR) |
955 |
{global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;} |
956 |
else if (newState == states::REVERSE_TIMEOUT_ERROR) |
957 |
{global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;} |
958 |
else if (newState == states::CALIBRATION_ERROR) |
959 |
{global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;} |
960 |
else if (newState == states::WHITE_DETECTION_ERROR) |
961 |
{global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;} |
962 |
else if (newState == states::PROXY_DETECTION_ERROR) |
963 |
{global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;} |
964 |
else if (newState == states::NO_CHARGING_POWER_ERROR) |
965 |
{global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;} |
966 |
else if (newState == states::UNKNOWN_STATE_ERROR) |
967 |
{global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;} |
968 |
} |
969 |
|
970 |
// Keep track of last state and set the new state for next iteration
|
971 |
prevState = currentState; |
972 |
currentState = newState; |
973 |
|
974 |
this->sleep(CAN::UPDATE_PERIOD);
|
975 |
} |
976 |
|
977 |
return RDY_OK;
|
978 |
} |