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
reckoner.cpp
- Committer:
- DavidEGrayson
- Date:
- 2014-02-23
- Revision:
- 12:835a4d24ae3b
- Child:
- 13:bba5b3abd13f
File content as of revision 12:835a4d24ae3b:
#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; }