David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
Diff: reckoner.cpp
- Revision:
- 36:ccb03b734737
- Parent:
- 21:c279c6a83671
- Child:
- 37:23000a47ed2b
diff -r d365d755c686 -r ccb03b734737 reckoner.cpp --- a/reckoner.cpp Wed Mar 05 04:43:36 2014 +0000 +++ b/reckoner.cpp Thu Mar 06 02:39:07 2014 +0000 @@ -153,12 +153,18 @@ void Reckoner::handleRight() { - cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; - sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; + handleTurnRadians(-DA); } void Reckoner::handleLeft() { - cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; - sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; + handleTurnRadians(DA); +} + +void Reckoner::handleTurnRadians(int32_t radians) +{ + int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE; + int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE; + cos += dc; + sin += ds; } \ No newline at end of file