David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: reckoner.cpp
- Revision:
- 12:835a4d24ae3b
- Child:
- 13:bba5b3abd13f
diff -r bd14d512340a -r 835a4d24ae3b reckoner.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/reckoner.cpp Sun Feb 23 22:23:34 2014 +0000 @@ -0,0 +1,77 @@ +#include <mbed.h> +#include "reckoner.h" + +/** +W: Wheel-to-wheel distance: 150 mm // TODO: correct this +G: Gear ratio factor: 30 +T: Encoder ticks per backshaft rotation: 12 (three-toothed encoder wheel) +R: Wheel radius: 70 mm + +Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T) +Df: Distance robot moves forward per encoder tick = Dw/2 +dA: Change in angle per encoder tick = Dw/W = 2*pi*70/(30*12) / 150 = 0.00814486984 +**/ + +#define LOG_UNIT_MAGNITUDE 30 + +#define DA 8745487 // 0.00814486984 * 0x40000000 + +#define LOG_COS_TO_X_CONVERSION 14 // 30 - 16 + +Reckoner reckoner; + +Reckoner::Reckoner() +{ + cos = 1 << LOG_UNIT_MAGNITUDE; + sin = 0; + x = 0; + y = 0; +} + +void Reckoner::handleTickLeftForward() +{ + handleForward(); + handleRight(); +} + +void Reckoner::handleTickLeftBackward() +{ + handleBackward(); + handleLeft(); +} + +void Reckoner::handleTickRightForward() +{ + handleForward(); + handleLeft(); +} + +void Reckoner::handleTickRightBackward() +{ + handleBackward(); + handleRight(); +} + +void Reckoner::handleForward() +{ + x += cos >> LOG_COS_TO_X_CONVERSION; + y += sin >> LOG_COS_TO_X_CONVERSION; +} + +void Reckoner::handleBackward() +{ + x -= cos >> LOG_COS_TO_X_CONVERSION; + y -= sin >> LOG_COS_TO_X_CONVERSION; +} + +void Reckoner::handleRight() +{ + cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; + sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; +} + +void Reckoner::handleLeft() +{ + cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; + sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; +} \ No newline at end of file