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.h@12:835a4d24ae3b, 2014-02-23 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Sun Feb 23 22:23:34 2014 +0000
- Revision:
- 12:835a4d24ae3b
- Child:
- 13:bba5b3abd13f
Made the Reckoner class and wrote a routine to help test it.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 12:835a4d24ae3b | 1 | // Robot configuration: |
DavidEGrayson | 12:835a4d24ae3b | 2 | |
DavidEGrayson | 12:835a4d24ae3b | 3 | // General purpose code: |
DavidEGrayson | 12:835a4d24ae3b | 4 | |
DavidEGrayson | 12:835a4d24ae3b | 5 | class Reckoner |
DavidEGrayson | 12:835a4d24ae3b | 6 | { |
DavidEGrayson | 12:835a4d24ae3b | 7 | public: |
DavidEGrayson | 12:835a4d24ae3b | 8 | |
DavidEGrayson | 12:835a4d24ae3b | 9 | Reckoner(); |
DavidEGrayson | 12:835a4d24ae3b | 10 | |
DavidEGrayson | 12:835a4d24ae3b | 11 | // Together, cos and sin form a vector with a magnitude close to 2^30 that indicates |
DavidEGrayson | 12:835a4d24ae3b | 12 | // the current position of the robot. By definition, when the reckoning starts, |
DavidEGrayson | 12:835a4d24ae3b | 13 | // sin is 0 and cos is 2^30. |
DavidEGrayson | 12:835a4d24ae3b | 14 | // cos corresponds to the x component of the orientation vector. |
DavidEGrayson | 12:835a4d24ae3b | 15 | // sin corresponds to the y component of the orientation vector. |
DavidEGrayson | 12:835a4d24ae3b | 16 | int32_t cos, sin; |
DavidEGrayson | 12:835a4d24ae3b | 17 | |
DavidEGrayson | 12:835a4d24ae3b | 18 | // Together, x and y are a vector that points from the starting point to the |
DavidEGrayson | 12:835a4d24ae3b | 19 | // robot's current position. |
DavidEGrayson | 12:835a4d24ae3b | 20 | // Units: |
DavidEGrayson | 12:835a4d24ae3b | 21 | // * If the units are too big, precision is lost. |
DavidEGrayson | 12:835a4d24ae3b | 22 | // * If they are too small, the integers wil overflow. |
DavidEGrayson | 12:835a4d24ae3b | 23 | // * For convenience, they should be a power of 2 off from Df (Distance robot moves |
DavidEGrayson | 12:835a4d24ae3b | 24 | // forward per encoder tick), so we can just write x += cos >> some_constant. |
DavidEGrayson | 12:835a4d24ae3b | 25 | // * Worst case for overflow: our encoders give us a Df of 0.25mm and our robot moves |
DavidEGrayson | 12:835a4d24ae3b | 26 | // 5m away from the starting point. Therefore, x and y might need to hold a value |
DavidEGrayson | 12:835a4d24ae3b | 27 | // 20000 or -20000 without overflowing. |
DavidEGrayson | 12:835a4d24ae3b | 28 | // Overflow condition: x = (1<<31) |
DavidEGrayson | 12:835a4d24ae3b | 29 | // 20000*Df < max_dist_x = (1<<31) * U |
DavidEGrayson | 12:835a4d24ae3b | 30 | // U > Df * (20000) / (1<<31) = Df / (1<<16) |
DavidEGrayson | 12:835a4d24ae3b | 31 | // * Therefore the units we choose for x are Df / (1<<16). |
DavidEGrayson | 12:835a4d24ae3b | 32 | // * If we wrote x += cos it would mean the units are Df / (1<<30). |
DavidEGrayson | 12:835a4d24ae3b | 33 | // Instead, we write x += cos >> 14 so the units are correct. |
DavidEGrayson | 12:835a4d24ae3b | 34 | int32_t x, y; |
DavidEGrayson | 12:835a4d24ae3b | 35 | |
DavidEGrayson | 12:835a4d24ae3b | 36 | void handleTickLeftForward(); |
DavidEGrayson | 12:835a4d24ae3b | 37 | void handleTickLeftBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 38 | void handleTickRightForward(); |
DavidEGrayson | 12:835a4d24ae3b | 39 | void handleTickRightBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 40 | void handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 41 | void handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 42 | void handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 43 | void handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 44 | }; |
DavidEGrayson | 12:835a4d24ae3b | 45 | |
DavidEGrayson | 12:835a4d24ae3b | 46 | extern Reckoner reckoner; |