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