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@24:fc01d9125d3b, 2014-02-28 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Fri Feb 28 01:07:14 2014 +0000
- Revision:
- 24:fc01d9125d3b
- Parent:
- 21:c279c6a83671
- Child:
- 36:ccb03b734737
Fixed the problem with LineTracker always thinking the line was visible.
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 | 12:835a4d24ae3b | 4 | /** |
DavidEGrayson | 13:bba5b3abd13f | 5 | First, we define two int32_t variables cos and sin that make a unit vector. |
DavidEGrayson | 13:bba5b3abd13f | 6 | These variables hold our current estimation of the direction the robot is pointed. |
DavidEGrayson | 13:bba5b3abd13f | 7 | We need to choose units for them that allow for a lot of accuracy without overflowing. |
DavidEGrayson | 13:bba5b3abd13f | 8 | We choose the units to be 1/(1 << 30) so that they will typically be about half of the |
DavidEGrayson | 13:bba5b3abd13f | 9 | way between 0 and overflow. |
DavidEGrayson | 13:bba5b3abd13f | 10 | The starting value ofvalue of the [cos, sin] will be [1 << 30, 0]. |
DavidEGrayson | 13:bba5b3abd13f | 11 | To record this choice, we define LOG_UNIT_MAGNITUDE: |
DavidEGrayson | 13:bba5b3abd13f | 12 | **/ |
DavidEGrayson | 13:bba5b3abd13f | 13 | #define LOG_UNIT_MAGNITUDE 30 |
DavidEGrayson | 13:bba5b3abd13f | 14 | |
DavidEGrayson | 13:bba5b3abd13f | 15 | /** |
DavidEGrayson | 13:bba5b3abd13f | 16 | We define dA to be the effect of a single encoder tick on the angle the robot is |
DavidEGrayson | 13:bba5b3abd13f | 17 | facing, in radians. This can be calculated from physical parameters of the robot. |
DavidEGrayson | 13:bba5b3abd13f | 18 | The value of dA will usually be less than 0.01. |
DavidEGrayson | 13:bba5b3abd13f | 19 | What we want to do to update cos and sin for a left-turning encoder tick is: |
DavidEGrayson | 13:bba5b3abd13f | 20 | |
DavidEGrayson | 13:bba5b3abd13f | 21 | cos -= sin * dA (floating point arithmetic) |
DavidEGrayson | 13:bba5b3abd13f | 22 | sin += cos * dA (floating point arithmetic) |
DavidEGrayson | 13:bba5b3abd13f | 23 | |
DavidEGrayson | 13:bba5b3abd13f | 24 | Instead of doing that we can do the equivalent using integer arithmetic. |
DavidEGrayson | 13:bba5b3abd13f | 25 | We define DA to be dA times (1 << LOG_UNIT_MAGNITUDE). |
DavidEGrayson | 13:bba5b3abd13f | 26 | Then we do: |
DavidEGrayson | 13:bba5b3abd13f | 27 | |
DavidEGrayson | 13:bba5b3abd13f | 28 | cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 13:bba5b3abd13f | 29 | sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 13:bba5b3abd13f | 30 | **/ |
DavidEGrayson | 13:bba5b3abd13f | 31 | |
DavidEGrayson | 13:bba5b3abd13f | 32 | /** |
DavidEGrayson | 13:bba5b3abd13f | 33 | We define Df to be the distance the robot moves forward per encoder tick. |
DavidEGrayson | 13:bba5b3abd13f | 34 | This is half of the amount a single wheel turns per encoder tick. |
DavidEGrayson | 13:bba5b3abd13f | 35 | Df is NOT an integer so we cannot store it in our program; it is a distance. |
DavidEGrayson | 13:bba5b3abd13f | 36 | We do not need to even know Df, because all of our distances can be |
DavidEGrayson | 13:bba5b3abd13f | 37 | measured and recorded in terms of Df. |
DavidEGrayson | 13:bba5b3abd13f | 38 | **/ |
DavidEGrayson | 13:bba5b3abd13f | 39 | |
DavidEGrayson | 13:bba5b3abd13f | 40 | /** |
DavidEGrayson | 13:bba5b3abd13f | 41 | We define two int32_t variables x and y to hold the current position of the robot. |
DavidEGrayson | 13:bba5b3abd13f | 42 | Together, x and y are a vector that points from the starting point to the robot's |
DavidEGrayson | 13:bba5b3abd13f | 43 | current position. |
DavidEGrayson | 13:bba5b3abd13f | 44 | |
DavidEGrayson | 13:bba5b3abd13f | 45 | When choosing the units of x and y, we have several considerations: |
DavidEGrayson | 13:bba5b3abd13f | 46 | * If the units are too big, precision is lost. |
DavidEGrayson | 13:bba5b3abd13f | 47 | * If the units are too small, the integers will overflow. |
DavidEGrayson | 13:bba5b3abd13f | 48 | * For convenience, the units should be a power of 2 off from Df (Distance robot moves |
DavidEGrayson | 13:bba5b3abd13f | 49 | forward per encoder tick), so we can just write lines like: |
DavidEGrayson | 13:bba5b3abd13f | 50 | x += cos >> some_constant |
DavidEGrayson | 13:bba5b3abd13f | 51 | |
DavidEGrayson | 13:bba5b3abd13f | 52 | Taking this into account, we choose the units of x and y to be Df / (1 << 14). |
DavidEGrayson | 13:bba5b3abd13f | 53 | |
DavidEGrayson | 13:bba5b3abd13f | 54 | Let's make sure this won't result in overflow: |
DavidEGrayson | 13:bba5b3abd13f | 55 | * To ensure no overflow happens, we need: |
DavidEGrayson | 13:bba5b3abd13f | 56 | (Maximum distance representable by x or y) > (Maximum roam of robot) |
DavidEGrayson | 13:bba5b3abd13f | 57 | (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) |
DavidEGrayson | 13:bba5b3abd13f | 58 | (Maximum roam of robot) / Df < (1 << 17) = 131072 |
DavidEGrayson | 13:bba5b3abd13f | 59 | * Assume Df is 0.1 mm (this is smaller than it will usually be for the robots we work with). |
DavidEGrayson | 13:bba5b3abd13f | 60 | * That means the robot can roam at most 13.1 m (0.0001 m * 131072) from its starting point, |
DavidEGrayson | 13:bba5b3abd13f | 61 | which should be plenty. |
DavidEGrayson | 13:bba5b3abd13f | 62 | |
DavidEGrayson | 13:bba5b3abd13f | 63 | So how do we update x and y? |
DavidEGrayson | 13:bba5b3abd13f | 64 | * When we update x and y, the position they represent changes by Df. |
DavidEGrayson | 13:bba5b3abd13f | 65 | * x and y represent a unit vector, but their units are 1 / (1 << LOG_UNIT_MAGNITUDE). |
DavidEGrayson | 13:bba5b3abd13f | 66 | * If we wrote x += cos it would mean the units of x are Df / (1 << LOG_UNIT_MAGNITUDE). |
DavidEGrayson | 13:bba5b3abd13f | 67 | (and x would overflow after 2 or 3 ticks). |
DavidEGrayson | 13:bba5b3abd13f | 68 | * Instead, we write x += cos >> 16 so the units are correct. |
DavidEGrayson | 13:bba5b3abd13f | 69 | **/ |
DavidEGrayson | 13:bba5b3abd13f | 70 | |
DavidEGrayson | 13:bba5b3abd13f | 71 | |
DavidEGrayson | 13:bba5b3abd13f | 72 | |
DavidEGrayson | 13:bba5b3abd13f | 73 | /** |
DavidEGrayson | 13:bba5b3abd13f | 74 | W: Wheel-to-wheel distance: 139 mm // Measured with calipers. |
DavidEGrayson | 12:835a4d24ae3b | 75 | G: Gear ratio factor: 30 |
DavidEGrayson | 12:835a4d24ae3b | 76 | T: Encoder ticks per backshaft rotation: 12 (three-toothed encoder wheel) |
DavidEGrayson | 13:bba5b3abd13f | 77 | R: Wheel radius: 35 mm (Pololu 70mm diameter wheel) |
DavidEGrayson | 12:835a4d24ae3b | 78 | |
DavidEGrayson | 12:835a4d24ae3b | 79 | Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T) |
DavidEGrayson | 12:835a4d24ae3b | 80 | Df: Distance robot moves forward per encoder tick = Dw/2 |
DavidEGrayson | 13:bba5b3abd13f | 81 | dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394 |
DavidEGrayson | 12:835a4d24ae3b | 82 | **/ |
DavidEGrayson | 12:835a4d24ae3b | 83 | |
DavidEGrayson | 14:c8cca3687e64 | 84 | /** The theoretical value for dA above turned out to not be so accurate. |
DavidEGrayson | 14:c8cca3687e64 | 85 | After playing with the robot for a few minutes, the robot was confused about which direction |
DavidEGrayson | 14:c8cca3687e64 | 86 | it was facing by about 30 degrees. |
DavidEGrayson | 15:4df8c50b5e91 | 87 | So I did an experiment to find out what dA really is. I turned the robot around many times and |
DavidEGrayson | 14:c8cca3687e64 | 88 | then took the difference in the encoder ticks from the left and right wheels. |
DavidEGrayson | 15:4df8c50b5e91 | 89 | dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks). |
DavidEGrayson | 14:c8cca3687e64 | 90 | The experiment was performed several times to see how accurate the number is. |
DavidEGrayson | 14:c8cca3687e64 | 91 | |
DavidEGrayson | 14:c8cca3687e64 | 92 | (Theoretical dA = 0.00439471394 ) |
DavidEGrayson | 14:c8cca3687e64 | 93 | Run 1: dA = 2*pi*15 / (11691 + 9414) = 0.00446566119 |
DavidEGrayson | 14:c8cca3687e64 | 94 | Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836 |
DavidEGrayson | 15:4df8c50b5e91 | 95 | Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874 |
DavidEGrayson | 15:4df8c50b5e91 | 96 | Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392 |
DavidEGrayson | 14:c8cca3687e64 | 97 | |
DavidEGrayson | 15:4df8c50b5e91 | 98 | The dA I decided to use is the average from runs 3 and 4: dA = 0.00446148633 |
DavidEGrayson | 15:4df8c50b5e91 | 99 | **/ |
DavidEGrayson | 15:4df8c50b5e91 | 100 | |
DavidEGrayson | 15:4df8c50b5e91 | 101 | #define DA 4790484 // 0.00446148633 * 0x40000000 |
DavidEGrayson | 12:835a4d24ae3b | 102 | |
DavidEGrayson | 13:bba5b3abd13f | 103 | #define LOG_COS_TO_X_CONVERSION 16 // 30 - 14 |
DavidEGrayson | 12:835a4d24ae3b | 104 | |
DavidEGrayson | 21:c279c6a83671 | 105 | Reckoner::Reckoner() |
DavidEGrayson | 21:c279c6a83671 | 106 | { |
DavidEGrayson | 21:c279c6a83671 | 107 | reset(); |
DavidEGrayson | 21:c279c6a83671 | 108 | } |
DavidEGrayson | 12:835a4d24ae3b | 109 | |
DavidEGrayson | 21:c279c6a83671 | 110 | void Reckoner::reset() |
DavidEGrayson | 12:835a4d24ae3b | 111 | { |
DavidEGrayson | 12:835a4d24ae3b | 112 | cos = 1 << LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 113 | sin = 0; |
DavidEGrayson | 12:835a4d24ae3b | 114 | x = 0; |
DavidEGrayson | 12:835a4d24ae3b | 115 | y = 0; |
DavidEGrayson | 12:835a4d24ae3b | 116 | } |
DavidEGrayson | 12:835a4d24ae3b | 117 | |
DavidEGrayson | 12:835a4d24ae3b | 118 | void Reckoner::handleTickLeftForward() |
DavidEGrayson | 12:835a4d24ae3b | 119 | { |
DavidEGrayson | 12:835a4d24ae3b | 120 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 121 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 122 | } |
DavidEGrayson | 12:835a4d24ae3b | 123 | |
DavidEGrayson | 12:835a4d24ae3b | 124 | void Reckoner::handleTickLeftBackward() |
DavidEGrayson | 12:835a4d24ae3b | 125 | { |
DavidEGrayson | 12:835a4d24ae3b | 126 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 127 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 128 | } |
DavidEGrayson | 12:835a4d24ae3b | 129 | |
DavidEGrayson | 12:835a4d24ae3b | 130 | void Reckoner::handleTickRightForward() |
DavidEGrayson | 12:835a4d24ae3b | 131 | { |
DavidEGrayson | 12:835a4d24ae3b | 132 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 133 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 134 | } |
DavidEGrayson | 12:835a4d24ae3b | 135 | |
DavidEGrayson | 12:835a4d24ae3b | 136 | void Reckoner::handleTickRightBackward() |
DavidEGrayson | 12:835a4d24ae3b | 137 | { |
DavidEGrayson | 12:835a4d24ae3b | 138 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 139 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 140 | } |
DavidEGrayson | 12:835a4d24ae3b | 141 | |
DavidEGrayson | 12:835a4d24ae3b | 142 | void Reckoner::handleForward() |
DavidEGrayson | 12:835a4d24ae3b | 143 | { |
DavidEGrayson | 12:835a4d24ae3b | 144 | x += cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 145 | y += sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 146 | } |
DavidEGrayson | 12:835a4d24ae3b | 147 | |
DavidEGrayson | 12:835a4d24ae3b | 148 | void Reckoner::handleBackward() |
DavidEGrayson | 12:835a4d24ae3b | 149 | { |
DavidEGrayson | 12:835a4d24ae3b | 150 | x -= cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 151 | y -= sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 152 | } |
DavidEGrayson | 12:835a4d24ae3b | 153 | |
DavidEGrayson | 12:835a4d24ae3b | 154 | void Reckoner::handleRight() |
DavidEGrayson | 12:835a4d24ae3b | 155 | { |
DavidEGrayson | 12:835a4d24ae3b | 156 | cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 157 | sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 158 | } |
DavidEGrayson | 12:835a4d24ae3b | 159 | |
DavidEGrayson | 12:835a4d24ae3b | 160 | void Reckoner::handleLeft() |
DavidEGrayson | 12:835a4d24ae3b | 161 | { |
DavidEGrayson | 12:835a4d24ae3b | 162 | cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 163 | sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 164 | } |