| 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;
|