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@21:c279c6a83671, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 23:20:34 2014 +0000
Revision:
21:c279c6a83671
Parent:
15:4df8c50b5e91
Child:
36:ccb03b734737
Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

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