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@13:bba5b3abd13f, 2014-02-23 (annotated)

Committer:
DavidEGrayson
Date:
Sun Feb 23 23:49:58 2014 +0000
Revision:
13:bba5b3abd13f
Parent:
12:835a4d24ae3b
Child:
14:c8cca3687e64
testReckoner routine works great!  I can roll my robot around and it always knows what quadrant it is pointing to and knows what quadrant it is in!

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 13:bba5b3abd13f 84 #define DA 4718788 // 0.00439471394 * 0x40000000
DavidEGrayson 12:835a4d24ae3b 85
DavidEGrayson 13:bba5b3abd13f 86 #define LOG_COS_TO_X_CONVERSION 16 // 30 - 14
DavidEGrayson 12:835a4d24ae3b 87
DavidEGrayson 12:835a4d24ae3b 88 Reckoner reckoner;
DavidEGrayson 12:835a4d24ae3b 89
DavidEGrayson 12:835a4d24ae3b 90 Reckoner::Reckoner()
DavidEGrayson 12:835a4d24ae3b 91 {
DavidEGrayson 12:835a4d24ae3b 92 cos = 1 << LOG_UNIT_MAGNITUDE;
DavidEGrayson 12:835a4d24ae3b 93 sin = 0;
DavidEGrayson 12:835a4d24ae3b 94 x = 0;
DavidEGrayson 12:835a4d24ae3b 95 y = 0;
DavidEGrayson 12:835a4d24ae3b 96 }
DavidEGrayson 12:835a4d24ae3b 97
DavidEGrayson 12:835a4d24ae3b 98 void Reckoner::handleTickLeftForward()
DavidEGrayson 12:835a4d24ae3b 99 {
DavidEGrayson 12:835a4d24ae3b 100 handleForward();
DavidEGrayson 12:835a4d24ae3b 101 handleRight();
DavidEGrayson 12:835a4d24ae3b 102 }
DavidEGrayson 12:835a4d24ae3b 103
DavidEGrayson 12:835a4d24ae3b 104 void Reckoner::handleTickLeftBackward()
DavidEGrayson 12:835a4d24ae3b 105 {
DavidEGrayson 12:835a4d24ae3b 106 handleBackward();
DavidEGrayson 12:835a4d24ae3b 107 handleLeft();
DavidEGrayson 12:835a4d24ae3b 108 }
DavidEGrayson 12:835a4d24ae3b 109
DavidEGrayson 12:835a4d24ae3b 110 void Reckoner::handleTickRightForward()
DavidEGrayson 12:835a4d24ae3b 111 {
DavidEGrayson 12:835a4d24ae3b 112 handleForward();
DavidEGrayson 12:835a4d24ae3b 113 handleLeft();
DavidEGrayson 12:835a4d24ae3b 114 }
DavidEGrayson 12:835a4d24ae3b 115
DavidEGrayson 12:835a4d24ae3b 116 void Reckoner::handleTickRightBackward()
DavidEGrayson 12:835a4d24ae3b 117 {
DavidEGrayson 12:835a4d24ae3b 118 handleBackward();
DavidEGrayson 12:835a4d24ae3b 119 handleRight();
DavidEGrayson 12:835a4d24ae3b 120 }
DavidEGrayson 12:835a4d24ae3b 121
DavidEGrayson 12:835a4d24ae3b 122 void Reckoner::handleForward()
DavidEGrayson 12:835a4d24ae3b 123 {
DavidEGrayson 12:835a4d24ae3b 124 x += cos >> LOG_COS_TO_X_CONVERSION;
DavidEGrayson 12:835a4d24ae3b 125 y += sin >> LOG_COS_TO_X_CONVERSION;
DavidEGrayson 12:835a4d24ae3b 126 }
DavidEGrayson 12:835a4d24ae3b 127
DavidEGrayson 12:835a4d24ae3b 128 void Reckoner::handleBackward()
DavidEGrayson 12:835a4d24ae3b 129 {
DavidEGrayson 12:835a4d24ae3b 130 x -= cos >> LOG_COS_TO_X_CONVERSION;
DavidEGrayson 12:835a4d24ae3b 131 y -= sin >> LOG_COS_TO_X_CONVERSION;
DavidEGrayson 12:835a4d24ae3b 132 }
DavidEGrayson 12:835a4d24ae3b 133
DavidEGrayson 12:835a4d24ae3b 134 void Reckoner::handleRight()
DavidEGrayson 12:835a4d24ae3b 135 {
DavidEGrayson 12:835a4d24ae3b 136 cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 12:835a4d24ae3b 137 sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 12:835a4d24ae3b 138 }
DavidEGrayson 12:835a4d24ae3b 139
DavidEGrayson 12:835a4d24ae3b 140 void Reckoner::handleLeft()
DavidEGrayson 12:835a4d24ae3b 141 {
DavidEGrayson 12:835a4d24ae3b 142 cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 12:835a4d24ae3b 143 sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
DavidEGrayson 12:835a4d24ae3b 144 }