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