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.h
- Revision:
- 12:835a4d24ae3b
- Child:
- 13:bba5b3abd13f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/reckoner.h Sun Feb 23 22:23:34 2014 +0000 @@ -0,0 +1,46 @@ +// Robot configuration: + +// General purpose code: + +class Reckoner +{ + public: + + Reckoner(); + + // Together, cos and sin form a vector with a magnitude close to 2^30 that indicates + // the current position of the robot. By definition, when the reckoning starts, + // sin is 0 and cos is 2^30. + // cos corresponds to the x component of the orientation vector. + // sin corresponds to the y component of the orientation vector. + int32_t cos, sin; + + // Together, x and y are a vector that points from the starting point to the + // robot's current position. + // Units: + // * If the units are too big, precision is lost. + // * If they are too small, the integers wil overflow. + // * For convenience, they should be a power of 2 off from Df (Distance robot moves + // forward per encoder tick), so we can just write x += cos >> some_constant. + // * Worst case for overflow: our encoders give us a Df of 0.25mm and our robot moves + // 5m away from the starting point. Therefore, x and y might need to hold a value + // 20000 or -20000 without overflowing. + // Overflow condition: x = (1<<31) + // 20000*Df < max_dist_x = (1<<31) * U + // U > Df * (20000) / (1<<31) = Df / (1<<16) + // * Therefore the units we choose for x are Df / (1<<16). + // * If we wrote x += cos it would mean the units are Df / (1<<30). + // Instead, we write x += cos >> 14 so the units are correct. + int32_t x, y; + + void handleTickLeftForward(); + void handleTickLeftBackward(); + void handleTickRightForward(); + void handleTickRightBackward(); + void handleForward(); + void handleBackward(); + void handleRight(); + void handleLeft(); +}; + +extern Reckoner reckoner; \ No newline at end of file