reckoner.cpp

00001 #include <mbed.h>
00002 #include "reckoner.h"
00003 #include "math.h"
00004 
00005 /**
00006 First, we define two int32_t variables cos and sin that make a unit vector.
00007 These variables hold our current estimation of the direction the robot is pointed.
00008 We need to choose units for them that allow for a lot of accuracy without overflowing.
00009 We choose the units to be 1/(1 << 30) so that they will typically be about half of the
00010 way between 0 and overflow.
00011 The starting value of the [cos, sin] will be [1 << 30, 0].
00012 To record this choice, we define LOG_UNIT_MAGNITUDE:
00013 **/
00014 #define LOG_UNIT_MAGNITUDE 30
00015 
00016 /**
00017 We define dA to be the effect of a single encoder tick on the angle the robot is
00018 facing, in radians.  This can be calculated from physical parameters of the robot.
00019 The value of dA will usually be less than 0.01.
00020 What we want to do to update cos and sin for a left-turning encoder tick is:
00021 
00022   cos -= sin * dA    (floating point arithmetic)
00023   sin += cos * dA    (floating point arithmetic)
00024 
00025 Instead of doing that we can do the equivalent using integer arithmetic.
00026 We define DA to be dA times (1 << LOG_UNIT_MAGNITUDE).
00027 Then we do:
00028 
00029   cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
00030   sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
00031 **/
00032 
00033 /**
00034 We define Df to be the distance the robot moves forward per encoder tick.
00035 This is half of the amount a single wheel turns per encoder tick.
00036 Df is NOT an integer so we cannot store it in our program; it is a distance.
00037 We do not need to even know Df, because all of our distances can be
00038 measured and recorded in terms of Df.
00039 **/
00040 
00041 /**
00042 We define two int32_t variables x and y to hold the current position of the robot.
00043 Together, x and y are a vector that points from the starting point to the robot's
00044 current position.
00045 
00046 When choosing the units of x and y, we have several considerations:
00047  * If the units are too big, precision is lost.
00048  * If the units are too small, the integers will overflow.
00049  * For convenience, the units should be a power of 2 off from Df (Distance robot moves
00050    forward per encoder tick), so we can just write lines like:
00051      x += cos >> some_constant
00052 
00053 Taking this into account, we choose the units of x and y to be Df / (1 << 14).
00054 
00055 Let's make sure this won't result in overflow:
00056  * To ensure no overflow happens, we need:
00057      (Maximum distance representable by x or y) > (Maximum roam of robot)
00058                      (1 << 31) * Df / (1 << 14) > (Maximum roam of robot)
00059                   (Maximum roam of robot) / Df  < (1 << 17) = 131072
00060  * Assume Df is 0.1 mm (this is smaller than it will usually be for the robots we work with).
00061  * That means the robot can roam at most 13.1 m (0.0001 m * 131072) from its starting point,
00062    which should be plenty.
00063 
00064 So how do we update x and y?
00065  * When we update x and y, the position they represent changes by Df.
00066  * x and y represent a unit vector, but their units are 1 / (1 << LOG_UNIT_MAGNITUDE).
00067  * If we wrote x += cos it would mean the units of x are Df / (1 << LOG_UNIT_MAGNITUDE).
00068    (and x would overflow after 2 or 3 ticks).
00069  * Instead, we write x += cos >> 16 so the units are correct.
00070 **/
00071 
00072 
00073 
00074 /**
00075 W: Wheel-to-wheel distance:              139 mm  // Measured with calipers.
00076 G: Gear ratio factor:                    30
00077 T: Encoder ticks per backshaft rotation: 12       (three-toothed encoder wheel)
00078 R: Wheel radius:                         35 mm    (Pololu 70mm diameter wheel)
00079 
00080 Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T)
00081 Df: Distance robot moves forward per encoder tick = Dw/2
00082 dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394
00083 **/
00084 
00085 /** The theoretical value for dA above turned out to not be so accurate.
00086 After playing with the robot for a few minutes, the robot was confused about which direction
00087 it was facing by about 30 degrees.
00088 So I did an experiment to find out what dA really is.  I turned the robot around many times and
00089 then took the difference in the encoder ticks from the left and right wheels.
00090 dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks).
00091 The experiment was performed several times to see how accurate the number is.
00092 
00093 (Theoretical dA                       = 0.00439471394 )
00094 Run 1: dA = 2*pi*15 / (11691 + 9414)  = 0.00446566119
00095 Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836
00096 Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874
00097 Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392
00098 
00099 The dA I decided to use is the average from runs 3 and 4:  dA = 0.00446148633
00100 **/
00101 
00102 #define DA 4790484  // 0.00446148633 * 0x40000000
00103 
00104 #define LOG_COS_TO_X_CONVERSION  16    // 30 - 14
00105 
00106 Reckoner::Reckoner()
00107 {
00108     reset();
00109 }
00110 
00111 void Reckoner::reset()
00112 {
00113   cosv = 1 << LOG_UNIT_MAGNITUDE;
00114   sinv = 0;
00115   x = 0;
00116   y = 0;
00117 }
00118     
00119 void Reckoner::handleTickLeftForward()
00120 {
00121     handleForward();
00122     handleRight();
00123 }
00124 
00125 void Reckoner::handleTickLeftBackward()
00126 {
00127     handleBackward();
00128     handleLeft();   
00129 }
00130 
00131 void Reckoner::handleTickRightForward()
00132 {
00133     handleForward();
00134     handleLeft();   
00135 }
00136 
00137 void Reckoner::handleTickRightBackward()
00138 {
00139     handleBackward();
00140     handleRight();   
00141 }
00142 
00143 void Reckoner::handleForward()
00144 {
00145     x += cosv >> LOG_COS_TO_X_CONVERSION;
00146     y += sinv >> LOG_COS_TO_X_CONVERSION;
00147 }
00148 
00149 void Reckoner::handleBackward()
00150 {
00151     x -= cosv >> LOG_COS_TO_X_CONVERSION;
00152     y -= sinv >> LOG_COS_TO_X_CONVERSION;
00153 }
00154 
00155 void Reckoner::handleRight()
00156 {
00157     // DA = 4790484
00158     // 0.2% boost
00159     handleTurnRadians(-4800065);
00160 }
00161 
00162 void Reckoner::handleLeft()
00163 {
00164     handleTurnRadians(DA);
00165 }
00166 
00167 void Reckoner::handleTurnRadians(int32_t radians)
00168 {
00169     int32_t dc = -((int64_t)sinv * radians) >> LOG_UNIT_MAGNITUDE;
00170     int32_t ds = ((int64_t)cosv * radians) >> LOG_UNIT_MAGNITUDE;
00171     dc = -((int64_t)(sinv+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
00172     ds = ((int64_t)(cosv+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
00173     cosv += dc;
00174     sinv += ds;
00175 }
00176 
00177 // For angle, 0x20000000 represents 45 degrees.
00178 void Reckoner::setTurnAngle(int32_t angle)
00179 {
00180     double angleToRadiansFactor = 1.46291808e-9;  // pi/4 / 0x20000000
00181     cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
00182     sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
00183 }