David Grayson / Mbed 2 deprecated DeadReckoning

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@38:5e93a479c244, 2014-03-13 (annotated)

Committer:
DavidEGrayson
Date:
Thu Mar 13 17:49:43 2014 +0000
Revision:
38:5e93a479c244
Parent:
37:23000a47ed2b
Child:
43:0e985a58f174
The final version of the code that was used for the competition.;

Who changed what in which revision?

UserRevisionLine numberNew 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 37:23000a47ed2b 156 // DA = 4790484
DavidEGrayson 37:23000a47ed2b 157 // 0.2% boost
DavidEGrayson 37:23000a47ed2b 158 handleTurnRadians(-4800065);
DavidEGrayson 12:835a4d24ae3b 159 }
DavidEGrayson 12:835a4d24ae3b 160
DavidEGrayson 12:835a4d24ae3b 161 void Reckoner::handleLeft()
DavidEGrayson 12:835a4d24ae3b 162 {
DavidEGrayson 36:ccb03b734737 163 handleTurnRadians(DA);
DavidEGrayson 36:ccb03b734737 164 }
DavidEGrayson 36:ccb03b734737 165
DavidEGrayson 36:ccb03b734737 166 void Reckoner::handleTurnRadians(int32_t radians)
DavidEGrayson 36:ccb03b734737 167 {
DavidEGrayson 36:ccb03b734737 168 int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 36:ccb03b734737 169 int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 37:23000a47ed2b 170 dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 37:23000a47ed2b 171 ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 36:ccb03b734737 172 cos += dc;
DavidEGrayson 36:ccb03b734737 173 sin += ds;
DavidEGrayson 12:835a4d24ae3b 174 }