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