Revision 10bf9cc0 devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
1 |
#include "userthread.hpp" |
|
1 |
// #include "userthread.hpp"
|
|
2 | 2 |
#include "global.hpp" |
3 |
#include <cmath> |
|
3 | 4 |
#include "linefollow.hpp" |
4 |
|
|
5 |
// #include <cmath> |
|
6 |
// #include "global.hpp" |
|
5 | 7 |
using namespace amiro; |
6 | 8 |
|
7 | 9 |
extern Global global; |
... | ... | |
51 | 53 |
lightAllLeds(Color::BLACK); |
52 | 54 |
} |
53 | 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 |
|
|
54 | 156 |
/** |
55 | 157 |
* Blocks as long as the position changes. |
56 | 158 |
*/ |
... | ... | |
67 | 169 |
global.robot.setLightColor(led % 8, Color(Color::BLACK)); |
68 | 170 |
led++; |
69 | 171 |
} |
172 |
lightAllLeds(Color::BLACK); |
|
70 | 173 |
} |
71 | 174 |
|
72 | 175 |
bool UserThread::checkFrontalObject(){ |
73 |
uint32_t thresh = 1000;
|
|
176 |
uint32_t thresh = pCtrl.threshMid;
|
|
74 | 177 |
uint32_t prox; |
75 | 178 |
for(int i=0; i<8; i++){ |
76 | 179 |
prox = global.robot.getProximityRingValue(i); |
... | ... | |
99 | 202 |
// setRpmSpeed(stop); |
100 | 203 |
checkForMotion(); |
101 | 204 |
int success = 0; |
102 |
global.odometry.resetPosition(); |
|
205 |
// global.odometry.resetPosition();
|
|
103 | 206 |
types::position start = global.startPos = global.odometry.getPosition(); |
104 | 207 |
global.motorcontrol.setMotorEnable(false); |
105 | 208 |
this->sleep(1000); |
106 | 209 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
107 | 210 |
|
108 | 211 |
// Amiro moved, docking was not successful |
109 |
if ((start.x + stop_.x) || (start.y + stop_.y)){ |
|
212 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){ |
|
213 |
if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){ |
|
110 | 214 |
lightAllLeds(Color::RED); |
111 | 215 |
// Enable Motor again if docking was not successful |
112 | 216 |
global.motorcontrol.setMotorEnable(true); |
... | ... | |
147 | 251 |
*/ |
148 | 252 |
// User thread state: |
149 | 253 |
|
150 |
|
|
151 |
// int whiteBuf = 0; |
|
152 |
// int proxyBuf = 0; |
|
153 |
// int reverseBuf = 0; |
|
154 |
int correctionStep = 0; |
|
155 |
// int dist_count = 0; |
|
156 |
// bool needDistance = false; |
|
157 |
|
|
158 |
uint16_t rProx[8]; // buffer for ring proxy values |
|
159 |
int rpmSpeed[2] = {0}; |
|
160 |
int stop[2] = {0}; |
|
161 |
int turn[2] = {5,-5}; |
|
162 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
163 | 254 |
for (uint8_t led = 0; led < 8; ++led) { |
164 | 255 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
165 | 256 |
} |
166 | 257 |
running = false; |
258 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
167 | 259 |
LineFollow lf(&global); |
168 | 260 |
/* |
169 | 261 |
* LOOP |
... | ... | |
177 | 269 |
|
178 | 270 |
if (accel_z < -900 /*-0.9g*/) { |
179 | 271 |
// Start line following when AMiRo is rotated |
180 |
if(utState == states::IDLE){
|
|
272 |
if(currentState == states::INACTIVE){
|
|
181 | 273 |
newState = states::FOLLOW_LINE; |
182 | 274 |
}else{ |
183 | 275 |
newState = states::IDLE; |
... | ... | |
191 | 283 |
global.msgReceived = false; |
192 | 284 |
// running = true; |
193 | 285 |
switch(global.lfStrategy){ |
194 |
case msg_content::START: |
|
195 |
newState = states::FOLLOW_LINE;
|
|
196 |
break;
|
|
197 |
case msg_content::STOP: |
|
286 |
case msg_content::MSG_START:
|
|
287 |
newState = states::CALIBRATION_CHECK;
|
|
288 |
break; |
|
289 |
case msg_content::MSG_STOP:
|
|
198 | 290 |
newState = states::IDLE; |
199 |
break;
|
|
200 |
case msg_content::EDGE_RIGHT: |
|
291 |
break; |
|
292 |
case msg_content::MSG_EDGE_RIGHT:
|
|
201 | 293 |
// newState = states::FOLLOW_LINE; |
202 | 294 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
203 |
break;
|
|
204 |
case msg_content::EDGE_LEFT: |
|
295 |
break; |
|
296 |
case msg_content::MSG_EDGE_LEFT:
|
|
205 | 297 |
// newState = states::FOLLOW_LINE; |
206 | 298 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
207 |
break;
|
|
208 |
case msg_content::FUZZY: |
|
299 |
break; |
|
300 |
case msg_content::MSG_FUZZY:
|
|
209 | 301 |
// newState = states::FOLLOW_LINE; |
210 | 302 |
lStrategy = LineFollowStrategy::FUZZY; |
211 |
break;
|
|
212 |
case msg_content::DOCK: |
|
303 |
break; |
|
304 |
case msg_content::MSG_DOCK:
|
|
213 | 305 |
newState = states::DETECT_STATION; |
214 |
break;
|
|
215 |
case msg_content::UNDOCK: |
|
306 |
break; |
|
307 |
case msg_content::MSG_UNDOCK:
|
|
216 | 308 |
newState = states::RELEASE; |
217 |
break;
|
|
218 |
case msg_content::CHARGE: |
|
309 |
break; |
|
310 |
case msg_content::MSG_CHARGE:
|
|
219 | 311 |
newState = states::CHARGING; |
220 |
break; |
|
312 |
break; |
|
313 |
case msg_content::MSG_RESET_ODOMETRY: |
|
314 |
global.odometry.resetPosition(); |
|
315 |
break; |
|
316 |
case msg_content::MSG_CALIBRATE_BLACK: |
|
317 |
proxCalib.calibrateBlack = true; |
|
318 |
// global.odometry.resetPosition(); |
|
319 |
newState = states::CALIBRATION; |
|
320 |
break; |
|
321 |
case msg_content::MSG_CALIBRATE_WHITE: |
|
322 |
proxCalib.calibrateBlack = false; |
|
323 |
newState = states::CALIBRATION; |
|
324 |
break; |
|
221 | 325 |
default: |
222 | 326 |
newState = states::IDLE; |
223 |
break;
|
|
327 |
break; |
|
224 | 328 |
} |
225 | 329 |
} |
226 |
// newState = utState;
|
|
330 |
// newState = currentState;
|
|
227 | 331 |
|
228 | 332 |
// Get sensor data |
229 | 333 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
... | ... | |
233 | 337 |
} |
234 | 338 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
235 | 339 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
236 |
switch(utState){ |
|
340 |
switch(currentState){ |
|
341 |
case states::INACTIVE: |
|
342 |
// Dummy state to deactivate every interaction |
|
343 |
break; |
|
344 |
// --------------------------------------- |
|
345 |
case states::CALIBRATION_CHECK: |
|
346 |
// global.robot.calibrate(); |
|
347 |
if(global.linePID.BThresh >= global.linePID.WThresh){ |
|
348 |
newState = states::CALIBRATION_ERROR; |
|
349 |
}else{ |
|
350 |
newState = states::FOLLOW_LINE; |
|
351 |
} |
|
352 |
break; |
|
353 |
// --------------------------------------- |
|
354 |
case states::CALIBRATION: |
|
355 |
/* Calibrate the global thresholds for black or white. |
|
356 |
This values will be used by the line follow object |
|
357 |
*/ |
|
358 |
|
|
359 |
proxCalib.buf = 0; |
|
360 |
if(proxCalib.calibrateBlack){ |
|
361 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n"); |
|
362 |
global.robot.calibrate(); |
|
363 |
} |
|
364 |
for(int i=0; i <= proxCalib.meanWindow; i++){ |
|
365 |
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
|
366 |
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
367 |
this->sleep(CAN::UPDATE_PERIOD); |
|
368 |
} |
|
369 |
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow); |
|
370 |
|
|
371 |
if(proxCalib.calibrateBlack){ |
|
372 |
global.linePID.BThresh = proxCalib.buf; |
|
373 |
}else { |
|
374 |
global.linePID.WThresh = proxCalib.buf; |
|
375 |
} |
|
376 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh); |
|
377 |
|
|
378 |
newState = states::IDLE; |
|
379 |
break; |
|
380 |
// --------------------------------------- |
|
237 | 381 |
case states::IDLE: |
238 | 382 |
global.motorcontrol.setMotorEnable(true); |
239 | 383 |
setRpmSpeed(stop); |
240 | 384 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
241 | 385 |
global.robot.requestCharging(0); |
242 | 386 |
} |
387 |
// pCtrl.pFactor = 0; |
|
388 |
pCtrl.staticCont = 0; |
|
243 | 389 |
utCount.whiteCount = 0; |
244 |
utCount.proxyCount = 0;
|
|
390 |
utCount.ringProxCount = 0;
|
|
245 | 391 |
utCount.errorCount = 0; |
246 |
utCount.idleCount++; |
|
247 |
if (utCount.idleCount > 10){ |
|
248 |
utCount.idleCount = 0; |
|
249 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
250 |
global.robot.transmitState(utState); |
|
251 |
} |
|
252 |
break; |
|
392 |
newState = states::INACTIVE; |
|
393 |
break; |
|
253 | 394 |
// --------------------------------------- |
254 | 395 |
case states::FOLLOW_LINE: |
255 | 396 |
// Set correct forward speed to every strategy |
... | ... | |
263 | 404 |
|
264 | 405 |
if(lf.followLine(rpmSpeed)){ |
265 | 406 |
utCount.whiteCount++; |
266 |
if(utCount.whiteCount >= WHITE_COUNT_THRESH){
|
|
267 |
rpmSpeed[0] = stop[0];
|
|
268 |
rpmSpeed[1] = stop[1];
|
|
269 |
newState = states::IDLE;
|
|
407 |
if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
|
408 |
setRpmSpeed(stop);
|
|
409 |
utCount.whiteCount = 0;
|
|
410 |
newState = states::WHITE_DETECTION_ERROR;
|
|
270 | 411 |
} |
271 | 412 |
}else{ |
272 | 413 |
utCount.whiteCount = 0; |
273 |
// setRpmSpeed(rpmSpeed); |
|
274 | 414 |
} |
275 | 415 |
|
276 |
if(getProxyRingSum() > PROXY_RING_THRESH){ |
|
277 |
utCount.proxyCount++; |
|
278 |
if(utCount.proxyCount > WHITE_COUNT_THRESH){ |
|
279 |
rpmSpeed[0] = stop[0]; |
|
280 |
rpmSpeed[1] = stop[1]; |
|
281 |
// newState = states::IDLE; |
|
282 |
if (continue_on_obstacle){ |
|
283 |
newState = states::TURN; |
|
284 |
utCount.proxyCount = 0; |
|
285 |
}else{ |
|
286 |
newState = states::IDLE; |
|
287 |
} |
|
288 |
} |
|
289 |
}else{ |
|
290 |
utCount.proxyCount = 0; |
|
291 |
} |
|
416 |
preventCollision(rpmSpeed, rProx); |
|
417 |
// proxSectorSpeedCorrection(rpmSpeed, rProx); |
|
292 | 418 |
|
293 |
if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
|
|
294 |
rpmSpeed[0] = stop[0];
|
|
295 |
rpmSpeed[1] = stop[1];
|
|
419 |
if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
|
420 |
utCount.ringProxCount = 0;
|
|
421 |
newState = states::TURN;
|
|
296 | 422 |
} |
297 | 423 |
|
298 | 424 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){ |
... | ... | |
302 | 428 |
setRpmSpeed(rpmSpeed); |
303 | 429 |
} |
304 | 430 |
|
305 |
break;
|
|
431 |
break; |
|
306 | 432 |
// --------------------------------------- |
307 | 433 |
case states::TURN: |
308 | 434 |
checkForMotion(); |
309 | 435 |
// Check if only front sensors are active |
310 | 436 |
if(checkFrontalObject()){ |
311 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
|
437 |
global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
|
|
312 | 438 |
// BaseThread::sleep(8000); |
313 | 439 |
checkForMotion(); |
314 | 440 |
newState = states::FOLLOW_LINE; |
315 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
|
441 |
// lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
|
316 | 442 |
}else{ |
317 |
newState = states::IDLE;
|
|
443 |
newState = states::PROXY_DETECTION_ERROR;
|
|
318 | 444 |
} |
319 |
break;
|
|
445 |
break; |
|
320 | 446 |
// --------------------------------------- |
321 | 447 |
case states::DETECT_STATION: |
322 | 448 |
if (global.forwardSpeed != DETECTION_SPEED){ |
... | ... | |
341 | 467 |
checkForMotion(); |
342 | 468 |
newState = states::CORRECT_POSITIONING; |
343 | 469 |
} |
344 |
break;
|
|
470 |
break; |
|
345 | 471 |
// --------------------------------------- |
346 | 472 |
case states::CORRECT_POSITIONING: |
347 | 473 |
if (global.forwardSpeed != CHARGING_SPEED){ |
... | ... | |
353 | 479 |
lf.followLine(rpmSpeed); |
354 | 480 |
setRpmSpeed(rpmSpeed); |
355 | 481 |
|
356 |
utCount.stepCount++;
|
|
357 |
if (utCount.stepCount >= MAX_CORRECTION_STEPS){
|
|
358 |
utCount.stepCount = 0;
|
|
482 |
utCount.stateTime++;
|
|
483 |
if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
|
484 |
utCount.stateTime = 0;
|
|
359 | 485 |
newState = states::REVERSE; |
360 | 486 |
setRpmSpeed(stop); |
361 | 487 |
checkForMotion(); |
362 | 488 |
} |
363 |
break;
|
|
489 |
break; |
|
364 | 490 |
// --------------------------------------- |
365 | 491 |
case states::REVERSE: |
366 | 492 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){ |
... | ... | |
368 | 494 |
} |
369 | 495 |
lf.followLine(rpmSpeed); |
370 | 496 |
setRpmSpeed(rpmSpeed); |
497 |
utCount.stateTime++; |
|
371 | 498 |
|
372 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
373 |
// Is of those sensors at it max val means that the AMiRo cant drive back |
|
374 |
// so check if correctly positioned |
|
375 |
//definitely wrong positioned -> correct position directly without rotation |
|
376 |
// Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate |
|
377 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
|
378 |
// setRpmSpeed(stop); |
|
379 |
// checkForMotion(); |
|
380 |
// newState = states::CHECK_POSITIONING; |
|
381 |
// } else |
|
382 | 499 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
383 | 500 |
// setRpmSpeed(stop); |
384 | 501 |
// checkForMotion(); |
385 |
utCount.stepCount = 0;
|
|
502 |
utCount.stateTime = 0;
|
|
386 | 503 |
newState = states::PUSH_BACK; |
504 |
}else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
|
505 |
setRpmSpeed(stop); |
|
506 |
utCount.stateTime = 0; |
|
507 |
utCount.errorCount++; |
|
508 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){ |
|
509 |
newState = states::DOCKING_ERROR; |
|
510 |
}else{ |
|
511 |
newState = states::CORRECT_POSITIONING; |
|
512 |
} |
|
387 | 513 |
} |
388 |
break; |
|
514 |
|
|
515 |
break; |
|
389 | 516 |
// --------------------------------------- |
390 | 517 |
case states::PUSH_BACK: |
391 | 518 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){ |
... | ... | |
394 | 521 |
lf.followLine(rpmSpeed); |
395 | 522 |
setRpmSpeed(rpmSpeed); |
396 | 523 |
|
397 |
utCount.stepCount++; |
|
398 |
if (utCount.stepCount > PUSH_BACK_COUNT){ |
|
524 |
utCount.stateTime++; |
|
525 |
if (utCount.stateTime > PUSH_BACK_TIMEOUT){ |
|
526 |
utCount.stateTime = 0; |
|
399 | 527 |
newState = states::CHECK_POSITIONING; |
400 | 528 |
} |
401 |
break;
|
|
529 |
break; |
|
402 | 530 |
// --------------------------------------- |
403 | 531 |
case states::CHECK_POSITIONING: |
404 | 532 |
setRpmSpeed(stop); |
405 | 533 |
checkForMotion(); |
406 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
|
407 | 534 |
if(checkDockingSuccess()){ |
408 | 535 |
newState = states::CHECK_VOLTAGE; |
409 | 536 |
}else{ |
410 | 537 |
utCount.errorCount++; |
411 | 538 |
newState = states::CORRECT_POSITIONING; |
412 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){ |
|
413 |
newState = states::ERROR; |
|
539 |
if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
|
540 |
newState = states::DOCKING_ERROR;
|
|
414 | 541 |
} |
415 | 542 |
} |
416 |
// } |
|
417 |
// else{ |
|
418 |
// newState = CORRECT_POSITIONING; |
|
419 |
// } |
|
420 |
break; |
|
543 |
break; |
|
421 | 544 |
// --------------------------------------- |
422 | 545 |
case states::CHECK_VOLTAGE: |
423 | 546 |
if(!checkPinEnabled()){ |
... | ... | |
425 | 548 |
} else { |
426 | 549 |
if(checkPinVoltage()){ |
427 | 550 |
// Pins are under voltage -> correctly docked |
428 |
utCount.errorCount = 0; |
|
551 |
|
|
429 | 552 |
newState = states::CHARGING; |
430 | 553 |
}else{ |
431 | 554 |
utCount.errorCount++; |
... | ... | |
441 | 564 |
} |
442 | 565 |
|
443 | 566 |
if (utCount.errorCount > DOCKING_ERROR_THRESH){ |
444 |
newState = states::ERROR; |
|
567 |
newState = states::DOCKING_ERROR;
|
|
445 | 568 |
} |
446 | 569 |
} |
447 | 570 |
} |
448 |
break;
|
|
571 |
break; |
|
449 | 572 |
// --------------------------------------- |
450 | 573 |
case states::RELEASE_TO_CORRECT: |
451 | 574 |
|
... | ... | |
458 | 581 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
459 | 582 |
checkForMotion(); |
460 | 583 |
|
461 |
global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);
|
|
584 |
global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
|
|
462 | 585 |
checkForMotion(); |
463 | 586 |
newState = states::CORRECT_POSITIONING; |
464 |
break; |
|
465 |
// --------------------------------------- |
|
466 |
case states::ERROR: |
|
467 |
utCount.errorCount = 0; |
|
468 |
// lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
469 |
newState = states::RELEASE; |
|
470 |
break; |
|
587 |
break; |
|
471 | 588 |
// --------------------------------------- |
472 | 589 |
case states::CHARGING: |
473 | 590 |
global.motorcontrol.setMotorEnable(false); |
591 |
utCount.errorCount = 0; |
|
474 | 592 |
// Formulate Request to enable charging |
475 | 593 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
476 | 594 |
global.robot.requestCharging(1); |
... | ... | |
478 | 596 |
if(checkPinEnabled()){ |
479 | 597 |
showChargingState(); |
480 | 598 |
} |
481 |
break; |
|
482 |
|
|
599 |
break; |
|
483 | 600 |
// --------------------------------------- |
484 | 601 |
case states::RELEASE: |
485 | 602 |
if (global.forwardSpeed != DETECTION_SPEED){ |
... | ... | |
497 | 614 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
498 | 615 |
checkForMotion(); |
499 | 616 |
// rotate back |
500 |
global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION); |
|
501 |
checkForMotion(); |
|
617 |
// global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
|
618 |
// checkForMotion();
|
|
502 | 619 |
|
503 | 620 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
504 | 621 |
// checkForMotion(); |
... | ... | |
509 | 626 |
// setRpmSpeed(rpmSpeed); |
510 | 627 |
} |
511 | 628 |
// lightAllLeds(Color::BLACK); |
512 |
break; |
|
513 |
|
|
629 |
break; |
|
630 |
// --------------------------------------- |
|
631 |
case states::DOCKING_ERROR: |
|
632 |
newState = states::RELEASE; |
|
633 |
break; |
|
634 |
// --------------------------------------- |
|
635 |
case states::REVERSE_TIMEOUT_ERROR: |
|
636 |
newState = states::IDLE; |
|
637 |
break; |
|
638 |
// --------------------------------------- |
|
639 |
case states::CALIBRATION_ERROR: |
|
640 |
newState = states::IDLE; |
|
641 |
break; |
|
642 |
// --------------------------------------- |
|
643 |
case states::WHITE_DETECTION_ERROR: |
|
644 |
newState = states::IDLE; |
|
645 |
break; |
|
646 |
// --------------------------------------- |
|
647 |
case states::PROXY_DETECTION_ERROR: |
|
648 |
newState = states::IDLE; |
|
649 |
break; |
|
650 |
// --------------------------------------- |
|
651 |
case states::NO_CHARGING_POWER_ERROR: |
|
652 |
newState = states::IDLE; |
|
653 |
break; |
|
654 |
// --------------------------------------- |
|
655 |
case states::UNKNOWN_STATE_ERROR: |
|
656 |
newState = states::IDLE; |
|
657 |
break; |
|
658 |
// --------------------------------------- |
|
514 | 659 |
default: |
515 |
break; |
|
660 |
newState = states::UNKNOWN_STATE_ERROR; |
|
661 |
break; |
|
516 | 662 |
} |
517 |
if (utState != newState){
|
|
663 |
if (currentState != newState){
|
|
518 | 664 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
519 | 665 |
global.robot.transmitState(newState); |
520 | 666 |
} |
521 |
utState = newState; |
|
667 |
prevState = currentState; |
|
668 |
currentState = newState; |
|
669 |
if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){ |
|
670 |
utCount.stateCount = 0; |
|
671 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
672 |
global.robot.transmitState(currentState); |
|
673 |
// global.robot.setOdometry(global.odometry.getPosition()); |
|
674 |
|
|
675 |
}else{ |
|
676 |
utCount.stateCount++; |
|
677 |
} |
|
522 | 678 |
this->sleep(CAN::UPDATE_PERIOD); |
523 | 679 |
} |
524 | 680 |
|
Also available in: Unified diff