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@48:597738b77f77, 2019-08-13 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Tue Aug 13 21:21:17 2019 +0000
- Revision:
- 48:597738b77f77
- Parent:
- 43:0e985a58f174
Changes from before the contest, I think.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 12:835a4d24ae3b | 1 | #include <mbed.h> |
DavidEGrayson | 12:835a4d24ae3b | 2 | #include "reckoner.h" |
DavidEGrayson | 12:835a4d24ae3b | 3 | |
DavidEGrayson | 43:0e985a58f174 | 4 | // We define Df to be the distance the robot moves forward per encoder tick. |
DavidEGrayson | 43:0e985a58f174 | 5 | // This is half of the amount a single wheel turns per encoder tick. |
DavidEGrayson | 43:0e985a58f174 | 6 | // Df is NOT an integer so we cannot store it in our program; it is a distance. |
DavidEGrayson | 43:0e985a58f174 | 7 | // We do not need to even know Df, because all of our distances can be |
DavidEGrayson | 43:0e985a58f174 | 8 | // measured and recorded in terms of Df. |
DavidEGrayson | 43:0e985a58f174 | 9 | // |
DavidEGrayson | 43:0e985a58f174 | 10 | // We define two int32_t variables x and y to hold the current position of the |
DavidEGrayson | 43:0e985a58f174 | 11 | // robot. Together, x and y are a vector that points from the starting point to |
DavidEGrayson | 43:0e985a58f174 | 12 | // the robot's current position. |
DavidEGrayson | 43:0e985a58f174 | 13 | // |
DavidEGrayson | 43:0e985a58f174 | 14 | // We choose the units of x and y to be Df / (1 << 14). |
DavidEGrayson | 43:0e985a58f174 | 15 | // |
DavidEGrayson | 43:0e985a58f174 | 16 | // Let's make sure this won't result in overflow: |
DavidEGrayson | 43:0e985a58f174 | 17 | // To ensure no overflow happens, we need: |
DavidEGrayson | 43:0e985a58f174 | 18 | // (Maximum distance representable by x or y) > (Maximum roam of robot) |
DavidEGrayson | 43:0e985a58f174 | 19 | // (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) |
DavidEGrayson | 43:0e985a58f174 | 20 | // (Maximum roam of robot) / Df < (1 << 17) = 131072 |
DavidEGrayson | 43:0e985a58f174 | 21 | // |
DavidEGrayson | 43:0e985a58f174 | 22 | // If we assume Df is 0.1 mm (which is pretty small), then this means the robot |
DavidEGrayson | 43:0e985a58f174 | 23 | // can roam at most 13.1 m (0.0001 m * 131072) from its starting point, |
DavidEGrayson | 43:0e985a58f174 | 24 | // which should be plenty. |
DavidEGrayson | 43:0e985a58f174 | 25 | // |
DavidEGrayson | 43:0e985a58f174 | 26 | // So how do we update x and y? |
DavidEGrayson | 43:0e985a58f174 | 27 | // We define two int32_t variables named cosv and sinv that are in the same |
DavidEGrayson | 43:0e985a58f174 | 28 | // units as x and y and represent a vector that points in the direction that |
DavidEGrayson | 43:0e985a58f174 | 29 | // the robot is pointing and has a magnitude of Df (i.e. 1 << 14). |
DavidEGrayson | 43:0e985a58f174 | 30 | // So we just do x += cosv and y += sinv to update x and y when the robot |
DavidEGrayson | 43:0e985a58f174 | 31 | // moves one encoder tick forward. |
DavidEGrayson | 12:835a4d24ae3b | 32 | |
DavidEGrayson | 21:c279c6a83671 | 33 | Reckoner::Reckoner() |
DavidEGrayson | 21:c279c6a83671 | 34 | { |
DavidEGrayson | 21:c279c6a83671 | 35 | reset(); |
DavidEGrayson | 21:c279c6a83671 | 36 | } |
DavidEGrayson | 12:835a4d24ae3b | 37 | |
DavidEGrayson | 21:c279c6a83671 | 38 | void Reckoner::reset() |
DavidEGrayson | 12:835a4d24ae3b | 39 | { |
DavidEGrayson | 43:0e985a58f174 | 40 | cosv = 1 << RECKONER_LOG_UNIT; |
DavidEGrayson | 43:0e985a58f174 | 41 | sinv = 0; |
DavidEGrayson | 43:0e985a58f174 | 42 | x = 0; |
DavidEGrayson | 43:0e985a58f174 | 43 | y = 0; |
DavidEGrayson | 12:835a4d24ae3b | 44 | } |
DavidEGrayson | 12:835a4d24ae3b | 45 | |
DavidEGrayson | 12:835a4d24ae3b | 46 | void Reckoner::handleForward() |
DavidEGrayson | 12:835a4d24ae3b | 47 | { |
DavidEGrayson | 43:0e985a58f174 | 48 | x += cosv; |
DavidEGrayson | 43:0e985a58f174 | 49 | y += sinv; |
DavidEGrayson | 12:835a4d24ae3b | 50 | } |
DavidEGrayson | 12:835a4d24ae3b | 51 | |
DavidEGrayson | 12:835a4d24ae3b | 52 | void Reckoner::handleBackward() |
DavidEGrayson | 12:835a4d24ae3b | 53 | { |
DavidEGrayson | 43:0e985a58f174 | 54 | x -= cosv; |
DavidEGrayson | 43:0e985a58f174 | 55 | y -= sinv; |
DavidEGrayson | 12:835a4d24ae3b | 56 | } |
DavidEGrayson | 12:835a4d24ae3b | 57 | |
DavidEGrayson | 43:0e985a58f174 | 58 | void Reckoner::setTurnAngle(int32_t angle) |
DavidEGrayson | 36:ccb03b734737 | 59 | { |
DavidEGrayson | 43:0e985a58f174 | 60 | // 0x20000000 represents 45 degrees. |
DavidEGrayson | 43:0e985a58f174 | 61 | const double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000 |
DavidEGrayson | 43:0e985a58f174 | 62 | cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); |
DavidEGrayson | 43:0e985a58f174 | 63 | sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); |
DavidEGrayson | 43:0e985a58f174 | 64 | } |