David Grayson / Mbed 2 deprecated LineFollowing

David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

reckoner.cpp@57:99bec7fab454, 2015-04-16 (annotated)

Committer:
DavidEGrayson
Date:
Thu Apr 16 22:00:15 2015 +0000
Revision:
57:99bec7fab454
Parent:
46:f11cb4f93aac 
Doubled the encoder counts for indicating the end of the course because we might have to start a little bit back from the finish line.

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 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 }