amiro-os / devices / DiWheelDrive / userthread.cpp @ 4d54a507
History | View | Annotate | Download (36.949 KB)
1 |
#include "userthread.hpp" |
---|---|
2 |
#include "amiro_map.hpp" |
3 |
#include "global.hpp" |
4 |
#include "linefollow.hpp" |
5 |
|
6 |
using namespace amiro; |
7 |
|
8 |
extern Global global;
|
9 |
|
10 |
// a buffer for the z-value of the accelerometer
|
11 |
int16_t accel_z; |
12 |
bool running = false; |
13 |
|
14 |
|
15 |
/**
|
16 |
* Set speed.
|
17 |
*
|
18 |
* @param rpmSpeed speed for left and right wheel in rounds/min
|
19 |
*/
|
20 |
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) { |
21 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
22 |
} |
23 |
|
24 |
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) { |
25 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
26 |
} |
27 |
|
28 |
void UserThread::lightOneLed(Color color, int idx){ |
29 |
global.robot.setLightColor(idx, Color(color)); |
30 |
} |
31 |
|
32 |
void UserThread::lightAllLeds(Color color){
|
33 |
int led = 0; |
34 |
for(led=0; led<8; led++){ |
35 |
lightOneLed(color, led); |
36 |
} |
37 |
} |
38 |
|
39 |
void UserThread::showChargingState(){
|
40 |
uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
41 |
Color color = Color::GREEN; |
42 |
if (numLeds <= 2){ |
43 |
color = Color::RED; |
44 |
}else if(numLeds <= 6){ |
45 |
color = Color::YELLOW; |
46 |
} |
47 |
for (int i=0; i<numLeds; i++){ |
48 |
lightOneLed(color, i); |
49 |
this->sleep(300); |
50 |
} |
51 |
this->sleep(1000); |
52 |
lightAllLeds(Color::BLACK); |
53 |
} |
54 |
|
55 |
void UserThread::chargeAsLED(){
|
56 |
uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
57 |
Color color = Color::GREEN; |
58 |
if (numLeds <= 2){ |
59 |
color = Color::RED; |
60 |
}else if(numLeds <= 6){ |
61 |
color = Color::YELLOW; |
62 |
} |
63 |
for (int i=0; i<numLeds; i++){ |
64 |
lightOneLed(color, i); |
65 |
// this->sleep(300);
|
66 |
} |
67 |
// this->sleep(1000);
|
68 |
// lightAllLeds(Color::BLACK);
|
69 |
} |
70 |
|
71 |
// ----------------------------------------------------------------
|
72 |
|
73 |
void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){ |
74 |
for (int i=0; i<8; i++){ |
75 |
sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8]; |
76 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
|
77 |
|
78 |
} |
79 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
|
80 |
|
81 |
} |
82 |
|
83 |
|
84 |
void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){ |
85 |
for (int i=2; i<5; i++){ |
86 |
sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax; |
87 |
} |
88 |
} |
89 |
|
90 |
void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){ |
91 |
int i;
|
92 |
uint16_t sProx[8];
|
93 |
int32_t sPMax = 0;
|
94 |
getProxySectorVals(proxVals, sProx); |
95 |
getMaxFrontSectorVal(sProx, sPMax); |
96 |
|
97 |
int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
|
98 |
int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
|
99 |
|
100 |
|
101 |
|
102 |
if(sPMax > pCtrl.threshMid){
|
103 |
rpmSpeed[0] = 0; |
104 |
rpmSpeed[1] = 0; |
105 |
pCtrl.staticCont++; |
106 |
}else if((speedL > 0) || (speedR > 0)){ |
107 |
pCtrl.staticCont = 0;
|
108 |
rpmSpeed[0] = speedL;
|
109 |
rpmSpeed[1] = speedR;
|
110 |
}else{
|
111 |
rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000); |
112 |
rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000); |
113 |
} |
114 |
|
115 |
for(i=4; i<5; i++){ |
116 |
if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){ |
117 |
rpmSpeed[0] = -5000000 ; |
118 |
rpmSpeed[1] = -5000000 ; |
119 |
// pCtrl.staticCont++;
|
120 |
break;
|
121 |
} |
122 |
} |
123 |
chargeAsLED(); |
124 |
|
125 |
// 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]);
|
126 |
|
127 |
|
128 |
} |
129 |
// -------------------------------------------------------------------
|
130 |
|
131 |
|
132 |
void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) { |
133 |
|
134 |
if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){ |
135 |
rpmSpeed[0] = rpmSpeed[0] / 2; |
136 |
rpmSpeed[1] = rpmSpeed[1] / 2; |
137 |
} |
138 |
|
139 |
if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){ |
140 |
rpmSpeed[0] = rpmSpeed[0] / 3; |
141 |
rpmSpeed[1] = rpmSpeed[1] / 3; |
142 |
} |
143 |
|
144 |
if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){ |
145 |
rpmSpeed[0] = 0; |
146 |
rpmSpeed[1] = 0; |
147 |
utCount.ringProxCount++; |
148 |
}else{
|
149 |
utCount.ringProxCount = 0;
|
150 |
} |
151 |
|
152 |
} |
153 |
|
154 |
|
155 |
/**
|
156 |
* Blocks as long as the position changes.
|
157 |
*/
|
158 |
void UserThread::checkForMotion(){
|
159 |
bool motion = true; |
160 |
int led = 0; |
161 |
types::position oldPos = global.odometry.getPosition(); |
162 |
while(motion){
|
163 |
this->sleep(200); |
164 |
types::position tmp = global.odometry.getPosition(); |
165 |
motion = oldPos.x != tmp.x; //abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
|
166 |
oldPos = tmp; |
167 |
global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
168 |
global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
169 |
led++; |
170 |
} |
171 |
lightAllLeds(Color::BLACK); |
172 |
} |
173 |
|
174 |
bool UserThread::checkFrontalObject(){
|
175 |
uint32_t thresh = pCtrl.threshMid; |
176 |
uint32_t prox; |
177 |
for(int i=0; i<8; i++){ |
178 |
prox = global.robot.getProximityRingValue(i); |
179 |
if((i == 3) || (i == 4)){ |
180 |
if(prox < thresh){
|
181 |
return false; |
182 |
} |
183 |
}else{
|
184 |
if(prox > thresh){
|
185 |
return false; |
186 |
} |
187 |
} |
188 |
} |
189 |
return true; |
190 |
} |
191 |
|
192 |
bool UserThread::checkPinVoltage(){
|
193 |
return global.ltc4412.isPluggedIn();
|
194 |
} |
195 |
|
196 |
bool UserThread::checkPinEnabled(){
|
197 |
return global.ltc4412.isEnabled();
|
198 |
} |
199 |
|
200 |
int UserThread::checkDockingSuccess(){
|
201 |
// setRpmSpeed(stop);
|
202 |
checkForMotion(); |
203 |
int success = 0; |
204 |
// global.odometry.resetPosition();
|
205 |
types::position start = global.startPos = global.odometry.getPosition(); |
206 |
global.motorcontrol.setMotorEnable(false);
|
207 |
this->sleep(1000); |
208 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
209 |
|
210 |
// Amiro moved, docking was not successful
|
211 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){
|
212 |
if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){ |
213 |
lightAllLeds(Color::RED); |
214 |
// Enable Motor again if docking was not successful
|
215 |
global.motorcontrol.setMotorEnable(true);
|
216 |
success = 0;
|
217 |
}else{
|
218 |
lightAllLeds(Color::GREEN); |
219 |
success = 1;
|
220 |
} |
221 |
|
222 |
// this->sleep(500);
|
223 |
lightAllLeds(Color::BLACK); |
224 |
return success;
|
225 |
} |
226 |
|
227 |
int UserThread::getProxyRingSum(){
|
228 |
int prox_sum = 0; |
229 |
for(int i=0; i<8;i++){ |
230 |
prox_sum += global.robot.getProximityRingValue(i);; |
231 |
} |
232 |
return prox_sum;
|
233 |
} |
234 |
|
235 |
int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){ |
236 |
int32_t diff = a - b; |
237 |
int32_t res = 0;
|
238 |
devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2); |
239 |
for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){ |
240 |
res += devCor.proxbuf[i]; |
241 |
} |
242 |
devCor.pCount++; |
243 |
devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW; |
244 |
|
245 |
devCor.currentDeviation = res / PROX_DEVIATION_MEAN_WINDOW; |
246 |
return devCor.currentDeviation;
|
247 |
} |
248 |
|
249 |
void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],
|
250 |
uint8_t id, uint8_t l, uint8_t r, uint8_t att) { |
251 |
map[id][0] = l;
|
252 |
map[id][1] = r;
|
253 |
map[id][2] = att;
|
254 |
} |
255 |
|
256 |
UserThread::UserThread() : |
257 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
258 |
{ |
259 |
} |
260 |
|
261 |
UserThread::~UserThread() |
262 |
{ |
263 |
} |
264 |
|
265 |
msg_t |
266 |
UserThread::main() |
267 |
{ |
268 |
/*
|
269 |
* SETUP
|
270 |
*/
|
271 |
// User thread state:
|
272 |
|
273 |
for (uint8_t led = 0; led < 8; ++led) { |
274 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
275 |
} |
276 |
|
277 |
// State Variables
|
278 |
ut_states prevState = ut_states::UT_IDLE; |
279 |
ut_states currentState = ut_states::UT_INACTIVE; |
280 |
ut_states newState = ut_states::UT_INACTIVE; |
281 |
|
282 |
|
283 |
running = false;
|
284 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
285 |
LineFollow lf(&global); |
286 |
AmiroMap map(&global, lStrategy); |
287 |
/*
|
288 |
* LOOP
|
289 |
*/
|
290 |
while (!this->shouldTerminate()) |
291 |
{ |
292 |
/*
|
293 |
* read accelerometer z-value
|
294 |
*/
|
295 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
296 |
|
297 |
if (accel_z < -900 /*-0.9g*/) { |
298 |
// Start line following when AMiRo is rotated
|
299 |
if(currentState == ut_states::UT_INACTIVE){
|
300 |
newState = ut_states::UT_FOLLOW_LINE; |
301 |
}else{
|
302 |
newState = ut_states::UT_IDLE; |
303 |
} |
304 |
lightAllLeds(Color::GREEN); |
305 |
this->sleep(1000); |
306 |
lightAllLeds(Color::BLACK); |
307 |
|
308 |
// If message was received handle it here:
|
309 |
} else if(global.msgReceived){ |
310 |
global.msgReceived = false;
|
311 |
// running = true;
|
312 |
switch(global.lfStrategy){
|
313 |
case msg_content::MSG_START:
|
314 |
newState = ut_states::UT_CALIBRATION_CHECK; |
315 |
break;
|
316 |
case msg_content::MSG_STOP:
|
317 |
newState = ut_states::UT_IDLE; |
318 |
break;
|
319 |
case msg_content::MSG_EDGE_RIGHT:
|
320 |
// newState = ut_states::UT_FOLLOW_LINE;
|
321 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
322 |
break;
|
323 |
case msg_content::MSG_EDGE_LEFT:
|
324 |
// newState = ut_states::UT_FOLLOW_LINE;
|
325 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
326 |
break;
|
327 |
case msg_content::MSG_FUZZY:
|
328 |
// newState = ut_states::UT_FOLLOW_LINE;
|
329 |
lStrategy = LineFollowStrategy::FUZZY; |
330 |
break;
|
331 |
case msg_content::MSG_DOCK:
|
332 |
newState = ut_states::UT_DETECT_STATION; |
333 |
break;
|
334 |
case msg_content::MSG_UNDOCK:
|
335 |
newState = ut_states::UT_RELEASE; |
336 |
break;
|
337 |
case msg_content::MSG_CHARGE:
|
338 |
newState = ut_states::UT_CHARGING; |
339 |
break;
|
340 |
case msg_content::MSG_RESET_ODOMETRY:
|
341 |
global.odometry.resetPosition(); |
342 |
break;
|
343 |
case msg_content::MSG_CALIBRATE_BLACK:
|
344 |
proxCalib.calibrateBlack = true;
|
345 |
// global.odometry.resetPosition();
|
346 |
newState = ut_states::UT_CALIBRATION; |
347 |
break;
|
348 |
case msg_content::MSG_CALIBRATE_WHITE:
|
349 |
proxCalib.calibrateBlack = false;
|
350 |
newState = ut_states::UT_CALIBRATION; |
351 |
break;
|
352 |
case msg_content::MSG_TEST_MAP_STATE:
|
353 |
if (AMIRO_MAP_AUTO_TRACKING){
|
354 |
newState = ut_states::UT_TEST_MAP_AUTO_STATE; |
355 |
}else {
|
356 |
newState = ut_states::UT_TEST_MAP_STATE; |
357 |
} |
358 |
break;
|
359 |
case msg_content::MSG_SET_DIST_THRESH:
|
360 |
|
361 |
|
362 |
break;
|
363 |
case msg_content::MSG_GET_MAP_INFO:
|
364 |
break;
|
365 |
default:
|
366 |
newState = ut_states::UT_IDLE; |
367 |
break;
|
368 |
} |
369 |
} |
370 |
// newState = currentState;
|
371 |
|
372 |
// Get sensor data
|
373 |
uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
374 |
uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
375 |
for(int i=0; i<8;i++){ |
376 |
rProx[i] = global.robot.getProximityRingValue(i); |
377 |
} |
378 |
|
379 |
// Continously update devication values
|
380 |
meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0); |
381 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
382 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
383 |
switch(currentState){
|
384 |
case ut_states::UT_INACTIVE:
|
385 |
// Dummy state to deactivate every interaction
|
386 |
break;
|
387 |
// ---------------------------------------
|
388 |
case ut_states::UT_CALIBRATION_CHECK:
|
389 |
// global.robot.calibrate();
|
390 |
if(global.linePID.BThresh >= global.linePID.WThresh){
|
391 |
newState = ut_states::UT_CALIBRATION_ERROR; |
392 |
}else{
|
393 |
newState = ut_states::UT_FOLLOW_LINE; |
394 |
} |
395 |
break;
|
396 |
// ---------------------------------------
|
397 |
case ut_states::UT_CALIBRATION:
|
398 |
/* Calibrate the global thresholds for black or white.
|
399 |
This values will be used by the line follow object
|
400 |
*/
|
401 |
|
402 |
proxCalib.buf = 0;
|
403 |
if(proxCalib.calibrateBlack){
|
404 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
|
405 |
global.robot.calibrate(); |
406 |
} |
407 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
408 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
409 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
410 |
this->sleep(CAN::UPDATE_PERIOD);
|
411 |
} |
412 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
|
413 |
|
414 |
if(proxCalib.calibrateBlack){
|
415 |
global.linePID.BThresh = proxCalib.buf; |
416 |
}else {
|
417 |
global.linePID.WThresh = proxCalib.buf; |
418 |
} |
419 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
|
420 |
|
421 |
newState = ut_states::UT_IDLE; |
422 |
break;
|
423 |
// ---------------------------------------
|
424 |
case ut_states::UT_IDLE:
|
425 |
global.motorcontrol.setMotorEnable(true);
|
426 |
setRpmSpeed(stop); |
427 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
428 |
global.robot.requestCharging(0);
|
429 |
} |
430 |
// pCtrl.pFactor = 0;
|
431 |
pCtrl.staticCont = 0;
|
432 |
utCount.whiteCount = 0;
|
433 |
utCount.ringProxCount = 0;
|
434 |
utCount.errorCount = 0;
|
435 |
newState = ut_states::UT_INACTIVE; |
436 |
break;
|
437 |
// ---------------------------------------
|
438 |
case ut_states::UT_FOLLOW_LINE:
|
439 |
// Set correct forward speed to every strategy
|
440 |
if (global.forwardSpeed != global.rpmForward[0]){ |
441 |
global.forwardSpeed = global.rpmForward[0];
|
442 |
} |
443 |
|
444 |
if(lf.getStrategy() != lStrategy){
|
445 |
lf.setStrategy(lStrategy); |
446 |
} |
447 |
|
448 |
if(lf.followLine(rpmSpeed)){
|
449 |
utCount.whiteCount++; |
450 |
if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
451 |
setRpmSpeed(stop); |
452 |
utCount.whiteCount = 0;
|
453 |
newState = ut_states::UT_WHITE_DETECTION_ERROR; |
454 |
} |
455 |
}else{
|
456 |
utCount.whiteCount = 0;
|
457 |
} |
458 |
|
459 |
preventCollision(rpmSpeed, rProx); |
460 |
// proxSectorSpeedCorrection(rpmSpeed, rProx);
|
461 |
|
462 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
463 |
utCount.ringProxCount = 0;
|
464 |
|
465 |
|
466 |
checkForMotion(); |
467 |
// Check if only front sensors are active
|
468 |
if (checkFrontalObject()) {
|
469 |
// global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
|
470 |
// // BaseThread::sleep(8000);
|
471 |
// checkForMotion();
|
472 |
this->utCount.whiteCount = 0; |
473 |
newState = ut_states::UT_TURN; |
474 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
475 |
} else {
|
476 |
newState = ut_states::UT_PROXY_DETECTION_ERROR; |
477 |
} |
478 |
} |
479 |
|
480 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
481 |
setRpmSpeedFuzzy(rpmSpeed); |
482 |
}else{
|
483 |
|
484 |
setRpmSpeed(rpmSpeed); |
485 |
} |
486 |
|
487 |
break;
|
488 |
// ---------------------------------------
|
489 |
case ut_states::UT_TURN:{
|
490 |
// Check the line strategy in order to continue driving on the right side
|
491 |
int factor = SPEED_CONVERSION_FACTOR;
|
492 |
int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
493 |
int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
494 |
int blackSensor = 0; |
495 |
if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
|
496 |
factor = -factor; |
497 |
blackSensor = frontL; |
498 |
}else{
|
499 |
blackSensor = frontR; |
500 |
} |
501 |
|
502 |
rpmSpeed[0] = factor * CHARGING_SPEED;
|
503 |
rpmSpeed[1] = -factor * CHARGING_SPEED;
|
504 |
setRpmSpeed(rpmSpeed); |
505 |
|
506 |
if ((blackSensor >= global.linePID.WThresh )){
|
507 |
utCount.whiteCount = 1;
|
508 |
}else {
|
509 |
if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){ |
510 |
utCount.whiteCount = 0;
|
511 |
newState = ut_states::UT_FOLLOW_LINE; |
512 |
setRpmSpeed(stop); |
513 |
} |
514 |
} |
515 |
break;
|
516 |
} |
517 |
// ---------------------------------------
|
518 |
case ut_states::UT_DETECT_STATION:
|
519 |
|
520 |
if (global.forwardSpeed != DETECTION_SPEED){
|
521 |
global.forwardSpeed = DETECTION_SPEED; |
522 |
} |
523 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
524 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
525 |
} |
526 |
|
527 |
lf.followLine(rpmSpeed); |
528 |
setRpmSpeed(rpmSpeed); |
529 |
// // Detect marker before docking station
|
530 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){
|
531 |
// Use proxy ring
|
532 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
533 |
|
534 |
setRpmSpeed(stop); |
535 |
checkForMotion(); |
536 |
// 180° Rotation
|
537 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
538 |
// BaseThread::sleep(8000);
|
539 |
checkForMotion(); |
540 |
newState = ut_states::UT_CORRECT_POSITIONING; |
541 |
} |
542 |
break;
|
543 |
// ---------------------------------------
|
544 |
case ut_states::UT_CORRECT_POSITIONING:
|
545 |
if (global.forwardSpeed != CHARGING_SPEED){
|
546 |
global.forwardSpeed = CHARGING_SPEED; |
547 |
} |
548 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
549 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
550 |
} |
551 |
lf.followLine(rpmSpeed); |
552 |
setRpmSpeed(rpmSpeed); |
553 |
|
554 |
utCount.stateTime++; |
555 |
if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
556 |
utCount.stateTime = 0;
|
557 |
newState = ut_states::UT_REVERSE; |
558 |
setRpmSpeed(stop); |
559 |
checkForMotion(); |
560 |
} |
561 |
break;
|
562 |
// ---------------------------------------
|
563 |
case ut_states::UT_REVERSE:
|
564 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
565 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
566 |
} |
567 |
lf.followLine(rpmSpeed); |
568 |
setRpmSpeed(rpmSpeed); |
569 |
// utCount.stateTime++;
|
570 |
|
571 |
// Docking is only successful if Deviation is in range and sensors are at their max values.
|
572 |
if((rProx[0] >= PROX_MAX_VAL) |
573 |
&& (rProx[7] >= PROX_MAX_VAL)
|
574 |
&& ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){ |
575 |
// setRpmSpeed(stop);
|
576 |
// checkForMotion();
|
577 |
utCount.stateTime = 0;
|
578 |
newState = ut_states::UT_PUSH_BACK; |
579 |
}else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
580 |
// Case R
|
581 |
utCount.stateTime = 0;
|
582 |
setRpmSpeed(stop); |
583 |
devCor.RCase = true;
|
584 |
lightAllLeds(Color::YELLOW); |
585 |
newState = ut_states::UT_DEVIATION_CORRECTION; |
586 |
}else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
587 |
// Case L
|
588 |
utCount.stateTime = 0;
|
589 |
setRpmSpeed(stop); |
590 |
devCor.RCase = false;
|
591 |
lightAllLeds(Color::WHITE); |
592 |
newState = ut_states::UT_DEVIATION_CORRECTION; |
593 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
594 |
setRpmSpeed(stop); |
595 |
utCount.stateTime = 0;
|
596 |
utCount.errorCount++; |
597 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
598 |
newState = ut_states::UT_DOCKING_ERROR; |
599 |
}else{
|
600 |
newState = ut_states::UT_CORRECT_POSITIONING; |
601 |
} |
602 |
} |
603 |
|
604 |
// if((devCor.currentDeviation <= -10)){
|
605 |
// rpmSpeed[0] -= 2000000;
|
606 |
// }else if(devCor.currentDeviation >= 10){
|
607 |
// rpmSpeed[1] -= 2000000;
|
608 |
// }
|
609 |
// setRpmSpeed(rpmSpeed);
|
610 |
break;
|
611 |
// ---------------------------------------
|
612 |
case ut_states::UT_DEVIATION_CORRECTION:
|
613 |
// if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
614 |
// lf.setStrategy(LineFollowStrategy::REVERSE);
|
615 |
// }
|
616 |
// lf.followLine(rpmSpeed);
|
617 |
// setRpmSpeed(rpmSpeed);
|
618 |
if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){ |
619 |
if(devCor.RCase){
|
620 |
rpmSpeed[0] = 0; |
621 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
622 |
}else {
|
623 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
624 |
rpmSpeed[1] = 0; |
625 |
} |
626 |
setRpmSpeed(rpmSpeed); |
627 |
}else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){ |
628 |
if(devCor.RCase){
|
629 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
630 |
rpmSpeed[1] = 0; |
631 |
}else {
|
632 |
rpmSpeed[0] = 0; |
633 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
634 |
} |
635 |
setRpmSpeed(rpmSpeed); |
636 |
if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){ |
637 |
utCount.stateTime = 0;
|
638 |
newState = ut_states::UT_REVERSE; |
639 |
setRpmSpeed(stop); |
640 |
} |
641 |
}else{
|
642 |
utCount.stateTime = 0;
|
643 |
newState = ut_states::UT_REVERSE; |
644 |
setRpmSpeed(stop); |
645 |
} |
646 |
|
647 |
utCount.stateTime++; |
648 |
|
649 |
|
650 |
// if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
651 |
// utCount.stateTime = 0;
|
652 |
// newState = ut_states::UT_CHECK_POSITIONING;
|
653 |
// }
|
654 |
break;
|
655 |
// ---------------------------------------
|
656 |
case ut_states::UT_PUSH_BACK:
|
657 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
658 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
659 |
} |
660 |
lf.followLine(rpmSpeed); |
661 |
setRpmSpeed(rpmSpeed); |
662 |
|
663 |
utCount.stateTime++; |
664 |
if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
665 |
utCount.stateTime = 0;
|
666 |
newState = ut_states::UT_CHECK_POSITIONING; |
667 |
} |
668 |
break;
|
669 |
// ---------------------------------------
|
670 |
case ut_states::UT_CHECK_POSITIONING:
|
671 |
setRpmSpeed(stop); |
672 |
checkForMotion(); |
673 |
if(checkDockingSuccess()){
|
674 |
newState = ut_states::UT_CHECK_VOLTAGE; |
675 |
}else{
|
676 |
utCount.errorCount++; |
677 |
newState = ut_states::UT_CORRECT_POSITIONING; |
678 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
679 |
newState = ut_states::UT_DOCKING_ERROR; |
680 |
} |
681 |
} |
682 |
break;
|
683 |
// ---------------------------------------
|
684 |
case ut_states::UT_CHECK_VOLTAGE:
|
685 |
if(!checkPinEnabled()){
|
686 |
global.robot.requestCharging(1);
|
687 |
} else {
|
688 |
if(checkPinVoltage()){
|
689 |
// Pins are under voltage -> correctly docked
|
690 |
|
691 |
newState = ut_states::UT_CHARGING; |
692 |
}else{
|
693 |
utCount.errorCount++; |
694 |
// No voltage on pins -> falsely docked
|
695 |
// deactivate pins
|
696 |
global.motorcontrol.setMotorEnable(true);
|
697 |
global.robot.requestCharging(0);
|
698 |
// TODO: Soft release when docking falsely
|
699 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
700 |
newState = ut_states::UT_RELEASE_TO_CORRECT; |
701 |
} else {
|
702 |
newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
|
703 |
} |
704 |
|
705 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){
|
706 |
newState = ut_states::UT_DOCKING_ERROR; |
707 |
} |
708 |
} |
709 |
} |
710 |
break;
|
711 |
// ---------------------------------------
|
712 |
case ut_states::UT_RELEASE_TO_CORRECT:
|
713 |
|
714 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
715 |
checkForMotion(); |
716 |
// move 1cm forward
|
717 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
718 |
checkForMotion(); |
719 |
// rotate back
|
720 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
721 |
checkForMotion(); |
722 |
|
723 |
global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION); |
724 |
checkForMotion(); |
725 |
newState = ut_states::UT_CORRECT_POSITIONING; |
726 |
break;
|
727 |
// ---------------------------------------
|
728 |
case ut_states::UT_CHARGING:
|
729 |
global.motorcontrol.setMotorEnable(false);
|
730 |
utCount.errorCount = 0;
|
731 |
// Formulate Request to enable charging
|
732 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
733 |
global.robot.requestCharging(1);
|
734 |
} |
735 |
if(checkPinEnabled()){
|
736 |
showChargingState(); |
737 |
} |
738 |
break;
|
739 |
// ---------------------------------------
|
740 |
case ut_states::UT_RELEASE:
|
741 |
|
742 |
if (global.forwardSpeed != DETECTION_SPEED){
|
743 |
global.rpmForward[0] = DETECTION_SPEED;
|
744 |
} |
745 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
746 |
global.robot.requestCharging(0);
|
747 |
}else{
|
748 |
global.motorcontrol.setMotorEnable(true);
|
749 |
// TODO: Use controlled
|
750 |
//Rotate -20° to free from magnet
|
751 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
752 |
checkForMotion(); |
753 |
// move 1cm forward
|
754 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
755 |
checkForMotion(); |
756 |
// rotate back
|
757 |
// global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
758 |
// checkForMotion();
|
759 |
|
760 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
761 |
// checkForMotion();
|
762 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
763 |
newState = ut_states::UT_FOLLOW_LINE; |
764 |
// whiteBuf = -100;
|
765 |
// lf.followLine(rpmSpeed);
|
766 |
// setRpmSpeed(rpmSpeed);
|
767 |
} |
768 |
// lightAllLeds(Color::BLACK);
|
769 |
break;
|
770 |
// ---------------------------------------
|
771 |
case ut_states::UT_DOCKING_ERROR:
|
772 |
newState = ut_states::UT_RELEASE; |
773 |
break;
|
774 |
// ---------------------------------------
|
775 |
case ut_states::UT_REVERSE_TIMEOUT_ERROR:
|
776 |
newState = ut_states::UT_IDLE; |
777 |
break;
|
778 |
// ---------------------------------------
|
779 |
case ut_states::UT_CALIBRATION_ERROR:
|
780 |
newState = ut_states::UT_IDLE; |
781 |
break;
|
782 |
// ---------------------------------------
|
783 |
case ut_states::UT_WHITE_DETECTION_ERROR:
|
784 |
newState = ut_states::UT_IDLE; |
785 |
break;
|
786 |
// ---------------------------------------
|
787 |
case ut_states::UT_PROXY_DETECTION_ERROR:
|
788 |
newState = ut_states::UT_IDLE; |
789 |
break;
|
790 |
// ---------------------------------------
|
791 |
case ut_states::UT_NO_CHARGING_POWER_ERROR:
|
792 |
newState = ut_states::UT_IDLE; |
793 |
break;
|
794 |
// ---------------------------------------
|
795 |
case ut_states::UT_UNKNOWN_STATE_ERROR:
|
796 |
newState = ut_states::UT_IDLE; |
797 |
break;
|
798 |
// ---------------------------------------
|
799 |
case ut_states::UT_TEST_MAP_AUTO_STATE:
|
800 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
801 |
"Start autotracking: \n");
|
802 |
|
803 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
804 |
"NodeCount: %d\n", map.getNodeCount());
|
805 |
for (int i=0; i<map.getNodeCount(); i++) { |
806 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
807 |
"ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id,
|
808 |
map.getNodeList()[i].left, map.getNodeList()[i].right); |
809 |
} |
810 |
// Start test suit for trackUpdate
|
811 |
map.reset(); |
812 |
global.tcase = 0;
|
813 |
newState = UT_TEST_MAP_STATE; |
814 |
break;
|
815 |
// --------------------------------------------------
|
816 |
case ut_states::UT_TEST_MAP_STATE:{
|
817 |
// Test suit for amiro map
|
818 |
|
819 |
|
820 |
|
821 |
// AmiroMap map = AmiroMap(&global);
|
822 |
|
823 |
// --------------------------------------------------
|
824 |
|
825 |
global.tcase++; |
826 |
// Set basic valid map configuration
|
827 |
setAttributes(global.testmap, 0, 1, 2, 1); |
828 |
setAttributes(global.testmap, 1, 2, 2, 0); |
829 |
setAttributes(global.testmap, 2, 1, 0, 0); |
830 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
831 |
chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());
|
832 |
global.testres[global.tcase] = map.get_state()->valid; |
833 |
|
834 |
global.tcase++; // 1
|
835 |
// Test map fail if first node is flagged with end
|
836 |
setAttributes(global.testmap, 0, 1, 2, 0xff); |
837 |
map.initialize(); |
838 |
global.testres[global.tcase] = !map.get_state()->valid; |
839 |
|
840 |
global.tcase++; // 2
|
841 |
// Test if node 2 is set as start node
|
842 |
setAttributes(global.testmap, 0, 1, 2, 0); |
843 |
setAttributes(global.testmap, 2, 1, 0, 1); |
844 |
map.initialize(); |
845 |
global.testres[global.tcase] = map.get_state()->current == 2;
|
846 |
|
847 |
global.tcase++; // 3
|
848 |
// Test if non reachable nodes will trigger invalid map
|
849 |
setAttributes(global.testmap, 3, 0, 0, 0); |
850 |
setAttributes(global.testmap, 4, 0, 0, 0xff); |
851 |
map.initialize(); |
852 |
global.testres[global.tcase] = !map.get_state()->valid; |
853 |
|
854 |
global.tcase++; // 4
|
855 |
// Test Reinitialization
|
856 |
setAttributes(global.testmap, 0, 1, 2, 1); |
857 |
setAttributes(global.testmap, 1, 2, 2, 0); |
858 |
setAttributes(global.testmap, 2, 1, 0, 0); |
859 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
860 |
map.initialize(); |
861 |
global.testres[global.tcase] = map.get_state()->valid; |
862 |
|
863 |
global.odometry.resetPosition(); |
864 |
uint8_t ret = 0;
|
865 |
global.tcase++; // 5
|
866 |
// Test update under normal linefollowing without fixpoint
|
867 |
ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
868 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
869 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
870 |
map.get_state()->current, map.get_state()->next); |
871 |
// No case should be true because neither was a node visited nor
|
872 |
// was a fixpoint detected.
|
873 |
global.testres[global.tcase] = (ret == 0x4);
|
874 |
|
875 |
|
876 |
global.odometry.setPosition(1.0, 0.0, 0.0); |
877 |
chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);
|
878 |
global.tcase++; // 6
|
879 |
// Fixpoint on left side
|
880 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
881 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
882 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
883 |
map.get_state()->current, map.get_state()->next); |
884 |
// No case should be true because neither was a node visited nor
|
885 |
// was a fixpoint detected.
|
886 |
// global.odometry
|
887 |
global.testres[global.tcase] = (ret == 0x2)
|
888 |
&& (map.get_state()->strategy == 0x02)
|
889 |
&& (map.get_state()->dist == 0)
|
890 |
&& (map.get_state()->current == 2);
|
891 |
|
892 |
|
893 |
global.odometry.setPosition(1.5, 0.0, 0.0); |
894 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
895 |
"Current Point: %d\n", global.odometry.getPosition().x);
|
896 |
global.tcase++; // 7
|
897 |
// Fixpoint on left side, no update should appear because fixpoint already
|
898 |
// marked
|
899 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
900 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
901 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
902 |
map.get_state()->current, map.get_state()->next); |
903 |
// No case should be true because neither was a node visited nor
|
904 |
// was a fixpoint detected.
|
905 |
global.testres[global.tcase] = (ret == 0x00)
|
906 |
&& (map.get_state()->strategy == 0x02);
|
907 |
// && (map.get_state()->dist == 0);
|
908 |
|
909 |
global.odometry.setPosition(1.2, 0.0, 0.0); |
910 |
global.tcase++; // 8
|
911 |
// Fixpoint on left side, no update should appear because fixpoint already
|
912 |
// marked
|
913 |
ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
914 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
915 |
"Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,
|
916 |
map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength); |
917 |
// No case should be true because neither was a node visited nor
|
918 |
// was a fixpoint detected.
|
919 |
global.testres[global.tcase] = |
920 |
(ret == 0x04) && (map.get_state()->strategy == 0x02); |
921 |
|
922 |
global.odometry.setPosition(.5, 0.0, 0.0); |
923 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
924 |
"Current Point: %d\n", global.odometry.getPosition().x);
|
925 |
global.tcase++; // 9
|
926 |
// Fixpoint on left side, no update should appear because fixpoint already
|
927 |
// marked
|
928 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
929 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
930 |
"Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
|
931 |
global.tcase, ret, map.get_state()->current, |
932 |
map.get_state()->next, map.get_state()->dist, |
933 |
map.get_state()->eLength); |
934 |
// No case should be true because neither was a node visited nor
|
935 |
// was a fixpoint detected.
|
936 |
global.testres[global.tcase] = |
937 |
(ret == 2) &&
|
938 |
(map.get_state()->strategy == 2) &&
|
939 |
// (map.get_state()->dist == 0) &&
|
940 |
(map.get_state()->eLength == 50);
|
941 |
|
942 |
global.odometry.setPosition(.75, 0.0, 0.0); |
943 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
944 |
"Current Point: %d\n", global.odometry.getPosition().x);
|
945 |
global.tcase++; // 10
|
946 |
// Fixpoint on left side, no update should appear because fixpoint already
|
947 |
// marked
|
948 |
ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
949 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
950 |
"Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
|
951 |
global.tcase, ret, map.get_state()->current, |
952 |
map.get_state()->next, map.get_state()->dist, |
953 |
map.get_state()->eLength); |
954 |
// No case should be true because neither was a node visited nor
|
955 |
// was a fixpoint detected.
|
956 |
global.testres[global.tcase] = |
957 |
(ret == 4) && (map.get_state()->strategy == 2) && |
958 |
// (map.get_state()->dist == 50)
|
959 |
(map.get_state()->eLength == 50);
|
960 |
|
961 |
int failed = 0; |
962 |
int passed = 0; |
963 |
for (int i = 0; i <= global.tcase; i++) { |
964 |
if (global.testres[i]) {
|
965 |
passed++; |
966 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
967 |
"Test %d Passed!\n", i);
|
968 |
} else {
|
969 |
failed++; |
970 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
971 |
"Test %d Failed\n", i);
|
972 |
} |
973 |
} |
974 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
975 |
"Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed, |
976 |
failed); |
977 |
|
978 |
newState = ut_states::UT_IDLE; |
979 |
break;
|
980 |
} |
981 |
// --------------------------------------------------
|
982 |
default:
|
983 |
newState = ut_states::UT_UNKNOWN_STATE_ERROR; |
984 |
break;
|
985 |
} |
986 |
|
987 |
// In case a new state is set:
|
988 |
// 1. Record the state transition
|
989 |
if (AMIRO_MAP_AUTO_TRACKING) {
|
990 |
// map.trackUpdate(WL, WR, lStrategy, currentState);
|
991 |
} |
992 |
|
993 |
if (currentState != newState){
|
994 |
|
995 |
global.stateTransitionCounter++; |
996 |
// Clear all state transitions to prevent overflow
|
997 |
if (global.stateTransitionCounter >= 255) { |
998 |
global.stateTransitionCounter = 0;
|
999 |
for (int i = 0; i < 24; i++) { |
1000 |
global.stateTracker[i] = 0;
|
1001 |
} |
1002 |
} |
1003 |
// Transmit the new state over can
|
1004 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
1005 |
global.robot.transmitState(newState); |
1006 |
|
1007 |
// Increase state count for specific state
|
1008 |
// TODO: Improve with dictionary or other than switch case
|
1009 |
if (newState == ut_states::UT_IDLE)
|
1010 |
{global.stateTracker[ut_states::UT_IDLE] += 1;}
|
1011 |
else if (newState == ut_states::UT_FOLLOW_LINE) |
1012 |
{global.stateTracker[ut_states::UT_FOLLOW_LINE] += 1;}
|
1013 |
else if (newState == ut_states::UT_DETECT_STATION) |
1014 |
{global.stateTracker[ut_states::UT_DETECT_STATION] += 1;}
|
1015 |
else if (newState == ut_states::UT_REVERSE) |
1016 |
{global.stateTracker[ut_states::UT_REVERSE] += 1;}
|
1017 |
else if (newState == ut_states::UT_PUSH_BACK) |
1018 |
{global.stateTracker[ut_states::UT_PUSH_BACK] += 1;}
|
1019 |
else if (newState == ut_states::UT_CHECK_POSITIONING) |
1020 |
{global.stateTracker[ut_states::UT_CHECK_POSITIONING] += 1;}
|
1021 |
else if (newState == ut_states::UT_CHECK_VOLTAGE) |
1022 |
{global.stateTracker[ut_states::UT_CHECK_VOLTAGE] += 1;}
|
1023 |
else if (newState == ut_states::UT_CHARGING) |
1024 |
{global.stateTracker[ut_states::UT_CHARGING] += 1;}
|
1025 |
else if (newState == ut_states::UT_RELEASE) |
1026 |
{global.stateTracker[ut_states::UT_RELEASE] += 1;}
|
1027 |
else if (newState == ut_states::UT_RELEASE_TO_CORRECT) |
1028 |
{global.stateTracker[ut_states::UT_RELEASE_TO_CORRECT] += 1;}
|
1029 |
else if (newState == ut_states::UT_CORRECT_POSITIONING) |
1030 |
{global.stateTracker[ut_states::UT_CORRECT_POSITIONING] += 1;}
|
1031 |
else if (newState == ut_states::UT_TURN) |
1032 |
{global.stateTracker[ut_states::UT_TURN] += 1;}
|
1033 |
else if (newState == ut_states::UT_INACTIVE) |
1034 |
{global.stateTracker[ut_states::UT_INACTIVE] += 1;}
|
1035 |
else if (newState == ut_states::UT_CALIBRATION) |
1036 |
{global.stateTracker[ut_states::UT_CALIBRATION] += 1;}
|
1037 |
else if (newState == ut_states::UT_CALIBRATION_CHECK) |
1038 |
{global.stateTracker[ut_states::UT_CALIBRATION_CHECK] += 1;}
|
1039 |
else if (newState == ut_states::UT_DEVIATION_CORRECTION) |
1040 |
{global.stateTracker[ut_states::UT_DEVIATION_CORRECTION] += 1;}
|
1041 |
else if (newState == ut_states::UT_DOCKING_ERROR) |
1042 |
{global.stateTracker[16+(-ut_states::UT_DOCKING_ERROR)] += 1;} |
1043 |
else if (newState == ut_states::UT_REVERSE_TIMEOUT_ERROR) |
1044 |
{global.stateTracker[16+(-ut_states::UT_REVERSE_TIMEOUT_ERROR)] += 1;} |
1045 |
else if (newState == ut_states::UT_CALIBRATION_ERROR) |
1046 |
{global.stateTracker[16+(-ut_states::UT_CALIBRATION_ERROR)] += 1;} |
1047 |
else if (newState == ut_states::UT_WHITE_DETECTION_ERROR) |
1048 |
{global.stateTracker[16+(-ut_states::UT_WHITE_DETECTION_ERROR)] += 1;} |
1049 |
else if (newState == ut_states::UT_PROXY_DETECTION_ERROR) |
1050 |
{global.stateTracker[16+(-ut_states::UT_PROXY_DETECTION_ERROR)] += 1;} |
1051 |
else if (newState == ut_states::UT_NO_CHARGING_POWER_ERROR) |
1052 |
{global.stateTracker[16+(-ut_states::UT_NO_CHARGING_POWER_ERROR)] += 1;} |
1053 |
else if (newState == ut_states::UT_UNKNOWN_STATE_ERROR) |
1054 |
{global.stateTracker[16+(-ut_states::UT_UNKNOWN_STATE_ERROR)] += 1;} |
1055 |
} |
1056 |
|
1057 |
// Keep track of last state and set the new state for next iteration
|
1058 |
prevState = currentState; |
1059 |
currentState = newState; |
1060 |
|
1061 |
this->sleep(CAN::UPDATE_PERIOD);
|
1062 |
} |
1063 |
|
1064 |
return RDY_OK;
|
1065 |
} |