David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
reckoner.cpp@14:c8cca3687e64, 2014-02-24 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Mon Feb 24 00:07:54 2014 +0000
- Revision:
- 14:c8cca3687e64
- Parent:
- 13:bba5b3abd13f
- Child:
- 15:4df8c50b5e91
Started doing an experiment to see what dA really is.
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 | 14:c8cca3687e64 | 87 | So I did an experiment to find out what dA really is. I turned the robot around 15 times and |
DavidEGrayson | 14:c8cca3687e64 | 88 | then took the difference in the encoder ticks from the left and right wheels. |
DavidEGrayson | 14:c8cca3687e64 | 89 | dA should be equal to 2*pi*numturns / (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 | 14:c8cca3687e64 | 95 | |
DavidEGrayson | 13:bba5b3abd13f | 96 | #define DA 4718788 // 0.00439471394 * 0x40000000 |
DavidEGrayson | 12:835a4d24ae3b | 97 | |
DavidEGrayson | 13:bba5b3abd13f | 98 | #define LOG_COS_TO_X_CONVERSION 16 // 30 - 14 |
DavidEGrayson | 12:835a4d24ae3b | 99 | |
DavidEGrayson | 12:835a4d24ae3b | 100 | Reckoner reckoner; |
DavidEGrayson | 12:835a4d24ae3b | 101 | |
DavidEGrayson | 12:835a4d24ae3b | 102 | Reckoner::Reckoner() |
DavidEGrayson | 12:835a4d24ae3b | 103 | { |
DavidEGrayson | 12:835a4d24ae3b | 104 | cos = 1 << LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 105 | sin = 0; |
DavidEGrayson | 12:835a4d24ae3b | 106 | x = 0; |
DavidEGrayson | 12:835a4d24ae3b | 107 | y = 0; |
DavidEGrayson | 12:835a4d24ae3b | 108 | } |
DavidEGrayson | 12:835a4d24ae3b | 109 | |
DavidEGrayson | 12:835a4d24ae3b | 110 | void Reckoner::handleTickLeftForward() |
DavidEGrayson | 12:835a4d24ae3b | 111 | { |
DavidEGrayson | 12:835a4d24ae3b | 112 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 113 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 114 | } |
DavidEGrayson | 12:835a4d24ae3b | 115 | |
DavidEGrayson | 12:835a4d24ae3b | 116 | void Reckoner::handleTickLeftBackward() |
DavidEGrayson | 12:835a4d24ae3b | 117 | { |
DavidEGrayson | 12:835a4d24ae3b | 118 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 119 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 120 | } |
DavidEGrayson | 12:835a4d24ae3b | 121 | |
DavidEGrayson | 12:835a4d24ae3b | 122 | void Reckoner::handleTickRightForward() |
DavidEGrayson | 12:835a4d24ae3b | 123 | { |
DavidEGrayson | 12:835a4d24ae3b | 124 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 125 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 126 | } |
DavidEGrayson | 12:835a4d24ae3b | 127 | |
DavidEGrayson | 12:835a4d24ae3b | 128 | void Reckoner::handleTickRightBackward() |
DavidEGrayson | 12:835a4d24ae3b | 129 | { |
DavidEGrayson | 12:835a4d24ae3b | 130 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 131 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 132 | } |
DavidEGrayson | 12:835a4d24ae3b | 133 | |
DavidEGrayson | 12:835a4d24ae3b | 134 | void Reckoner::handleForward() |
DavidEGrayson | 12:835a4d24ae3b | 135 | { |
DavidEGrayson | 12:835a4d24ae3b | 136 | x += cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 137 | y += sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 138 | } |
DavidEGrayson | 12:835a4d24ae3b | 139 | |
DavidEGrayson | 12:835a4d24ae3b | 140 | void Reckoner::handleBackward() |
DavidEGrayson | 12:835a4d24ae3b | 141 | { |
DavidEGrayson | 12:835a4d24ae3b | 142 | x -= cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 143 | y -= sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 144 | } |
DavidEGrayson | 12:835a4d24ae3b | 145 | |
DavidEGrayson | 12:835a4d24ae3b | 146 | void Reckoner::handleRight() |
DavidEGrayson | 12:835a4d24ae3b | 147 | { |
DavidEGrayson | 12:835a4d24ae3b | 148 | cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 149 | sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 150 | } |
DavidEGrayson | 12:835a4d24ae3b | 151 | |
DavidEGrayson | 12:835a4d24ae3b | 152 | void Reckoner::handleLeft() |
DavidEGrayson | 12:835a4d24ae3b | 153 | { |
DavidEGrayson | 12:835a4d24ae3b | 154 | cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 155 | sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 156 | } |