Revision 2330e415 devices/DiWheelDrive/userthread.cpp
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 108 | 108 |
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
|
| 109 | 109 |
target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
| 110 | 110 |
target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
| 111 |
chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]); |
|
| 111 |
// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
|
|
| 112 | 112 |
} |
| 113 | 113 |
|
| 114 | 114 |
// Fuzzyfication of the sensor values |
| ... | ... | |
| 311 | 311 |
// Hard deviatio to right |
| 312 | 312 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
| 313 | 313 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
|
| 314 |
copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
|
|
| 314 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
|
|
| 315 | 315 |
// Hard deviation to left |
| 316 | 316 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
| 317 | 317 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
|
| 318 |
copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
|
|
| 319 |
// Turn if white
|
|
| 318 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
|
|
| 319 |
// stop if white
|
|
| 320 | 320 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE |
| 321 | 321 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
|
| 322 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
|
|
| 322 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
|
|
| 323 | 323 |
} |
| 324 | 324 |
} |
| 325 | 325 |
|
| ... | ... | |
| 452 | 452 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 453 | 453 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 454 | 454 |
} |
| 455 |
lf.followLine(vcnl4020Proximity, &global); |
|
| 455 |
lf.followLine(vcnl4020Proximity, rpmFuzzyCtrl, &global);
|
|
| 456 | 456 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n", |
| 457 | 457 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
| 458 | 458 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| ... | ... | |
| 461 | 461 |
//if (configLineFollowing==2) |
| 462 | 462 |
// lineFollownew |
| 463 | 463 |
//else |
| 464 |
lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
|
|
| 465 |
setRpmSpeed(rpmFuzzyCtrl); |
|
| 464 |
// lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
|
|
| 465 |
// setRpmSpeed(rpmFuzzyCtrl);
|
|
| 466 | 466 |
} |
| 467 | 467 |
|
| 468 |
// this->sleep(US2ST(5)); |
|
| 468 | 469 |
this->sleep(CAN::UPDATE_PERIOD); |
| 469 | 470 |
} |
| 470 | 471 |
|
| 471 | 472 |
return RDY_OK; |
| 472 | 473 |
} |
| 473 |
|
|
Also available in: Unified diff