Revision 61544eee devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
80 | 80 |
global.odometry.resetPosition(); |
81 | 81 |
types::position start = global.startPos = global.odometry.getPosition(); |
82 | 82 |
global.motorcontrol.toggleMotorEnable(); |
83 |
this->sleep(1000);
|
|
83 |
this->sleep(500);
|
|
84 | 84 |
types::position stop_ = global.endPos = global.odometry.getPosition(); |
85 | 85 |
|
86 | 86 |
// Amiro moved, docking was not successful |
... | ... | |
94 | 94 |
success = 1; |
95 | 95 |
} |
96 | 96 |
|
97 |
this->sleep(500); |
|
97 |
// this->sleep(500);
|
|
98 | 98 |
lightAllLeds(Color::BLACK); |
99 | 99 |
return success; |
100 | 100 |
} |
... | ... | |
129 | 129 |
|
130 | 130 |
int whiteBuf = 0; |
131 | 131 |
int proxyBuf = 0; |
132 |
int releaseBuf = 0; |
|
132 | 133 |
int correctionStep = 0; |
134 |
int dist_count = 0; |
|
135 |
bool needDistance = false; |
|
136 |
|
|
133 | 137 |
uint16_t rProx[8]; // buffer for ring proxy values |
134 | 138 |
int rpmSpeed[2] = {0}; |
135 | 139 |
int stop[2] = {0}; |
140 |
int turn[2] = {5,-5}; |
|
136 | 141 |
LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY; |
137 | 142 |
for (uint8_t led = 0; led < 8; ++led) { |
138 | 143 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
... | ... | |
216 | 221 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
217 | 222 |
global.robot.requestCharging(0); |
218 | 223 |
} |
219 |
|
|
224 |
whiteBuf = 0; |
|
225 |
proxyBuf = 0; |
|
220 | 226 |
break; |
221 | 227 |
// --------------------------------------- |
222 | 228 |
case states::FOLLOW_LINE: |
... | ... | |
231 | 237 |
|
232 | 238 |
//TODO: Check if white is detected and stop threshold is reached |
233 | 239 |
if(lf.followLine(rpmSpeed)){ |
234 |
|
|
240 |
whiteBuf++; |
|
235 | 241 |
if(whiteBuf >= WHITE_COUNT_THRESH){ |
236 |
setRpmSpeed(stop); |
|
242 |
rpmSpeed[0] = stop[0]; |
|
243 |
rpmSpeed[1] = stop[1]; |
|
237 | 244 |
newState = states::IDLE; |
238 |
}else{ |
|
239 |
whiteBuf++; |
|
240 | 245 |
} |
241 | 246 |
}else{ |
242 | 247 |
whiteBuf = 0; |
243 |
setRpmSpeed(rpmSpeed); |
|
248 |
// setRpmSpeed(rpmSpeed);
|
|
244 | 249 |
} |
245 | 250 |
|
246 | 251 |
if(getProxyRingSum() > PROXY_RING_THRESH){ |
247 |
setRpmSpeed(stop); |
|
248 | 252 |
proxyBuf++; |
249 | 253 |
if(proxyBuf > WHITE_COUNT_THRESH){ |
250 | 254 |
newState = states::IDLE; |
255 |
rpmSpeed[0] = stop[0]; |
|
256 |
rpmSpeed[1] = stop[1]; |
|
251 | 257 |
} |
252 | 258 |
}else{ |
253 | 259 |
proxyBuf = 0; |
254 |
} |
|
260 |
} |
|
261 |
|
|
262 |
if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){ |
|
263 |
rpmSpeed[0] = stop[0]; |
|
264 |
rpmSpeed[1] = stop[1]; |
|
265 |
} |
|
266 |
|
|
267 |
// if (needDistance){ |
|
268 |
// whiteBuf = 0; |
|
269 |
// proxyBuf = 0; |
|
270 |
// dist_count++; |
|
271 |
// if(dist_count > DIST_THRESH){ |
|
272 |
// dist_count = 0; |
|
273 |
// needDistance = false; |
|
274 |
// } |
|
275 |
// } |
|
276 |
// whiteBuf = 0; |
|
277 |
// proxyBuf = 0; |
|
255 | 278 |
// lf.followLine(rpmSpeed); |
256 |
// setRpmSpeed(rpmSpeed);
|
|
279 |
setRpmSpeed(rpmSpeed); |
|
257 | 280 |
|
258 | 281 |
break; |
259 | 282 |
// --------------------------------------- |
260 | 283 |
case states::DETECT_STATION: |
261 |
// if (global.forwardSpeed != CHARGING_SPEED){
|
|
262 |
// global.forwardSpeed = CHARGING_SPEED;
|
|
263 |
// }
|
|
284 |
if (global.forwardSpeed != DETECTION_SPEED){
|
|
285 |
global.forwardSpeed = DETECTION_SPEED;
|
|
286 |
} |
|
264 | 287 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){ |
265 | 288 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
266 | 289 |
} |
... | ... | |
312 | 335 |
// Is of those sensors at it max val means that the AMiRo cant drive back |
313 | 336 |
// so check if correctly positioned |
314 | 337 |
//definitely wrong positioned -> correct position directly without rotation |
315 |
if ((rProx[0] >= PROX_MAX_VAL && rProx[7] < PROX_MAX_VAL) || (rProx[0] < PROX_MAX_VAL && rProx[7] >= PROX_MAX_VAL)){ |
|
316 |
setRpmSpeed(stop); |
|
317 |
checkForMotion(); |
|
318 |
newState = states::CORRECT_POSITIONING; |
|
319 |
} |
|
320 | 338 |
// Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate |
339 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
|
340 |
// setRpmSpeed(stop); |
|
341 |
// checkForMotion(); |
|
342 |
// newState = states::CHECK_POSITIONING; |
|
343 |
// } else |
|
321 | 344 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
322 |
setRpmSpeed(stop); |
|
323 |
checkForMotion(); |
|
345 |
// setRpmSpeed(stop);
|
|
346 |
// checkForMotion();
|
|
324 | 347 |
newState = states::CHECK_POSITIONING; |
325 | 348 |
} |
326 | 349 |
break; |
327 | 350 |
// --------------------------------------- |
328 | 351 |
case states::CHECK_POSITIONING: |
329 |
if(checkDockingSuccess()){ |
|
330 |
newState = states::CHECK_VOLTAGE; |
|
331 |
}else{ |
|
332 |
newState = states::CORRECT_POSITIONING; |
|
333 |
} |
|
352 |
setRpmSpeed(stop); |
|
353 |
checkForMotion(); |
|
354 |
// if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
|
355 |
if(checkDockingSuccess()){ |
|
356 |
newState = states::CHECK_VOLTAGE; |
|
357 |
}else{ |
|
358 |
newState = states::CORRECT_POSITIONING; |
|
359 |
} |
|
360 |
// } |
|
361 |
// else{ |
|
362 |
// newState = CORRECT_POSITIONING; |
|
363 |
// } |
|
334 | 364 |
break; |
335 | 365 |
// --------------------------------------- |
336 | 366 |
case states::CHECK_VOLTAGE: |
... | ... | |
344 | 374 |
// No voltage on pins -> falsely docked |
345 | 375 |
// deactivate pins |
346 | 376 |
global.robot.requestCharging(0); |
347 |
newState = states::RELEASE_TO_CORRECT; |
|
377 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
|
378 |
newState = states::RELEASE_TO_CORRECT; |
|
379 |
} else { |
|
380 |
newState = states::CORRECT_POSITIONING; |
|
381 |
} |
|
348 | 382 |
} |
349 | 383 |
} |
350 | 384 |
break; |
... | ... | |
379 | 413 |
|
380 | 414 |
// --------------------------------------- |
381 | 415 |
case states::RELEASE: |
416 |
if (global.forwardSpeed != DETECTION_SPEED){ |
|
417 |
global.rpmForward[0] = DETECTION_SPEED; |
|
418 |
} |
|
382 | 419 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
383 | 420 |
global.robot.requestCharging(0); |
384 |
} |
|
385 |
|
|
386 |
if(checkPinEnabled()){ |
|
387 |
showChargingState(); |
|
388 | 421 |
}else{ |
389 | 422 |
if (!global.motorcontrol.getMotorEnable()){ |
390 | 423 |
global.motorcontrol.toggleMotorEnable(); |
391 | 424 |
} |
425 |
// if (releaseBuf > RELEASE_COUNT){ |
|
426 |
// releaseBuf = 0; |
|
427 |
// setRpmSpeed(stop); |
|
428 |
// needDistance = true; |
|
429 |
// newState = states::FOLLOW_LINE; |
|
430 |
// }else{ |
|
431 |
// releaseBuf++; |
|
432 |
// setRpmSpeed(turn); |
|
433 |
// } |
|
392 | 434 |
//Rotate -20° to free from magnet |
393 | 435 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
394 |
checkForMotion(); |
|
395 |
// move 1cm forward |
|
396 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
|
397 |
checkForMotion(); |
|
398 |
// rotate back |
|
399 |
global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION); |
|
400 |
checkForMotion(); |
|
436 |
checkForMotion();
|
|
437 |
// move 1cm forward
|
|
438 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
|
439 |
checkForMotion();
|
|
440 |
// rotate back
|
|
441 |
global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
|
442 |
checkForMotion();
|
|
401 | 443 |
|
402 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
|
403 |
checkForMotion(); |
|
444 |
// global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
|
445 |
// checkForMotion();
|
|
404 | 446 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
405 | 447 |
newState = states::FOLLOW_LINE; |
406 |
whiteBuf = -100; |
|
448 |
// whiteBuf = -100; |
|
449 |
// lf.followLine(rpmSpeed); |
|
450 |
// setRpmSpeed(rpmSpeed); |
|
407 | 451 |
} |
408 |
lightAllLeds(Color::BLACK); |
|
452 |
// lightAllLeds(Color::BLACK);
|
|
409 | 453 |
break; |
410 | 454 |
|
411 | 455 |
default: |
Also available in: Unified diff