Revision 019224ff devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
130 | 130 |
int whiteBuf = 0; |
131 | 131 |
int proxyBuf = 0; |
132 | 132 |
int correctionStep = 0; |
133 |
uint16_t rProx[8]; // buffer for ring proxy values |
|
133 | 134 |
int rpmSpeed[2] = {0}; |
134 | 135 |
int stop[2] = {0}; |
135 | 136 |
LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY; |
... | ... | |
199 | 200 |
newState = utState; |
200 | 201 |
|
201 | 202 |
// Get sensor data |
202 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
203 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
|
203 |
uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
204 |
uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
|
205 |
for(int i=0; i<8;i++){ |
|
206 |
rProx[i] = global.robot.getProximityRingValue(i); |
|
207 |
} |
|
204 | 208 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
205 | 209 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
206 | 210 |
switch(utState){ |
... | ... | |
254 | 258 |
break; |
255 | 259 |
// --------------------------------------- |
256 | 260 |
case states::DETECT_STATION: |
257 |
if (global.forwardSpeed != CHARGING_SPEED){ |
|
258 |
global.forwardSpeed = CHARGING_SPEED; |
|
259 |
} |
|
261 |
// if (global.forwardSpeed != CHARGING_SPEED){
|
|
262 |
// global.forwardSpeed = CHARGING_SPEED;
|
|
263 |
// }
|
|
260 | 264 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){ |
261 | 265 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
262 | 266 |
} |
... | ... | |
264 | 268 |
lf.followLine(rpmSpeed); |
265 | 269 |
setRpmSpeed(rpmSpeed); |
266 | 270 |
// // Detect marker before docking station |
267 |
if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
271 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
272 |
// Use proxy ring |
|
273 |
if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
|
274 |
|
|
268 | 275 |
setRpmSpeed(stop); |
269 | 276 |
checkForMotion(); |
270 | 277 |
// 180° Rotation |
... | ... | |
276 | 283 |
break; |
277 | 284 |
// --------------------------------------- |
278 | 285 |
case states::CORRECT_POSITIONING: |
286 |
if (global.forwardSpeed != CHARGING_SPEED){ |
|
287 |
global.forwardSpeed = CHARGING_SPEED; |
|
288 |
} |
|
279 | 289 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){ |
280 | 290 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
281 | 291 |
} |
... | ... | |
298 | 308 |
lf.followLine(rpmSpeed); |
299 | 309 |
setRpmSpeed(rpmSpeed); |
300 | 310 |
|
301 |
if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
311 |
// if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
312 |
// Is of those sensors at it max val means that the AMiRo cant drive back |
|
313 |
// so check if correctly positioned |
|
314 |
//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 |
// Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate |
|
321 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
|
302 | 322 |
setRpmSpeed(stop); |
303 | 323 |
checkForMotion(); |
304 | 324 |
newState = states::CHECK_POSITIONING; |
... | ... | |
324 | 344 |
// No voltage on pins -> falsely docked |
325 | 345 |
// deactivate pins |
326 | 346 |
global.robot.requestCharging(0); |
327 |
newState = states::CORRECT_POSITIONING;
|
|
347 |
newState = states::RELEASE_TO_CORRECT;
|
|
328 | 348 |
} |
329 | 349 |
} |
350 |
break; |
|
351 |
// --------------------------------------- |
|
352 |
case states::RELEASE_TO_CORRECT: |
|
353 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
|
354 |
checkForMotion(); |
|
355 |
// move 1cm forward |
|
356 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
|
357 |
checkForMotion(); |
|
358 |
// rotate back |
|
359 |
global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
|
360 |
checkForMotion(); |
|
361 |
|
|
362 |
global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION); |
|
363 |
checkForMotion(); |
|
364 |
newState = states::CORRECT_POSITIONING; |
|
365 |
break; |
|
330 | 366 |
// --------------------------------------- |
331 | 367 |
case states::CHARGING: |
332 | 368 |
if (global.motorcontrol.getMotorEnable()){ |
... | ... | |
355 | 391 |
} |
356 | 392 |
//Rotate -20° to free from magnet |
357 | 393 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
358 |
checkForMotion(); |
|
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(); |
|
401 |
|
|
402 |
global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
|
403 |
checkForMotion(); |
|
359 | 404 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
360 | 405 |
newState = states::FOLLOW_LINE; |
406 |
whiteBuf = -100; |
|
361 | 407 |
} |
362 | 408 |
lightAllLeds(Color::BLACK); |
363 | 409 |
break; |
Also available in: Unified diff