Diff: reckoner.cpp

Revision:

43:0e985a58f174

Parent:

37:23000a47ed2b

--- a/reckoner.cpp	Sat Jul 27 20:58:46 2019 +0000
+++ b/reckoner.cpp	Sat Jul 27 22:52:19 2019 +0000
@@ -1,106 +1,34 @@
 #include <mbed.h>
 #include "reckoner.h"
 
-/**
-First, we define two int32_t variables cos and sin that make a unit vector.
-These variables hold our current estimation of the direction the robot is pointed.
-We need to choose units for them that allow for a lot of accuracy without overflowing.
-We choose the units to be 1/(1 << 30) so that they will typically be about half of the
-way between 0 and overflow.
-The starting value ofvalue of the [cos, sin] will be [1 << 30, 0].
-To record this choice, we define LOG_UNIT_MAGNITUDE:
-**/
-#define LOG_UNIT_MAGNITUDE 30
-
-/**
-We define dA to be the effect of a single encoder tick on the angle the robot is
-facing, in radians.  This can be calculated from physical parameters of the robot.
-The value of dA will usually be less than 0.01.
-What we want to do to update cos and sin for a left-turning encoder tick is:
-
-  cos -= sin * dA    (floating point arithmetic)
-  sin += cos * dA    (floating point arithmetic)
-
-Instead of doing that we can do the equivalent using integer arithmetic.
-We define DA to be dA times (1 << LOG_UNIT_MAGNITUDE).
-Then we do:
-
-  cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
-  sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
-**/
-
-/**
-We define Df to be the distance the robot moves forward per encoder tick.
-This is half of the amount a single wheel turns per encoder tick.
-Df is NOT an integer so we cannot store it in our program; it is a distance.
-We do not need to even know Df, because all of our distances can be
-measured and recorded in terms of Df.
-**/
-
-/**
-We define two int32_t variables x and y to hold the current position of the robot.
-Together, x and y are a vector that points from the starting point to the robot's
-current position.
-
-When choosing the units of x and y, we have several considerations:
- * If the units are too big, precision is lost.
- * If the units are too small, the integers will overflow.
- * For convenience, the units should be a power of 2 off from Df (Distance robot moves
-   forward per encoder tick), so we can just write lines like:
-     x += cos >> some_constant
-
-Taking this into account, we choose the units of x and y to be Df / (1 << 14).
-
-Let's make sure this won't result in overflow:
- * To ensure no overflow happens, we need:
-     (Maximum distance representable by x or y) > (Maximum roam of robot)
-                     (1 << 31) * Df / (1 << 14) > (Maximum roam of robot)
-                  (Maximum roam of robot) / Df  < (1 << 17) = 131072
- * Assume Df is 0.1 mm (this is smaller than it will usually be for the robots we work with).
- * That means the robot can roam at most 13.1 m (0.0001 m * 131072) from its starting point,
-   which should be plenty.
-
-So how do we update x and y?
- * When we update x and y, the position they represent changes by Df.
- * x and y represent a unit vector, but their units are 1 / (1 << LOG_UNIT_MAGNITUDE).
- * If we wrote x += cos it would mean the units of x are Df / (1 << LOG_UNIT_MAGNITUDE).
-   (and x would overflow after 2 or 3 ticks).
- * Instead, we write x += cos >> 16 so the units are correct.
-**/
-
-
-
-/**
-W: Wheel-to-wheel distance:              139 mm  // Measured with calipers.
-G: Gear ratio factor:                    30
-T: Encoder ticks per backshaft rotation: 12       (three-toothed encoder wheel)
-R: Wheel radius:                         35 mm    (Pololu 70mm diameter wheel)
-
-Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T)
-Df: Distance robot moves forward per encoder tick = Dw/2
-dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394
-**/
-
-/** The theoretical value for dA above turned out to not be so accurate.
-After playing with the robot for a few minutes, the robot was confused about which direction
-it was facing by about 30 degrees.
-So I did an experiment to find out what dA really is.  I turned the robot around many times and
-then took the difference in the encoder ticks from the left and right wheels.
-dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks).
-The experiment was performed several times to see how accurate the number is.
-
-(Theoretical dA                       = 0.00439471394 )
-Run 1: dA = 2*pi*15 / (11691 + 9414)  = 0.00446566119
-Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836
-Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874
-Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392
-
-The dA I decided to use is the average from runs 3 and 4:  dA = 0.00446148633
-**/
-
-#define DA 4790484  // 0.00446148633 * 0x40000000
-
-#define LOG_COS_TO_X_CONVERSION  16    // 30 - 14
+// We define Df to be the distance the robot moves forward per encoder tick.
+// This is half of the amount a single wheel turns per encoder tick.
+// Df is NOT an integer so we cannot store it in our program; it is a distance.
+// We do not need to even know Df, because all of our distances can be
+// measured and recorded in terms of Df.
+// 
+// We define two int32_t variables x and y to hold the current position of the
+// robot.  Together, x and y are a vector that points from the starting point to
+// the robot's current position.
+//
+// We choose the units of x and y to be Df / (1 << 14).
+//
+// Let's make sure this won't result in overflow:
+// To ensure no overflow happens, we need:
+//   (Maximum distance representable by x or y) > (Maximum roam of robot)
+//                   (1 << 31) * Df / (1 << 14) > (Maximum roam of robot)
+//                (Maximum roam of robot) / Df  < (1 << 17) = 131072
+//
+// If we assume Df is 0.1 mm (which is pretty small), then this means the robot
+// can roam at most 13.1 m (0.0001 m * 131072) from its starting point,
+// which should be plenty.
+//
+// So how do we update x and y?
+// We define two int32_t variables named cosv and sinv that are in the same
+// units as x and y and represent a vector that points in the direction that
+// the robot is pointing and has a magnitude of Df (i.e. 1 << 14).
+// So we just do x += cosv and y += sinv to update x and y when the robot
+// moves one encoder tick forward.
 
 Reckoner::Reckoner()
 {
@@ -109,66 +37,28 @@
 
 void Reckoner::reset()
 {
-  cos = 1 << LOG_UNIT_MAGNITUDE;
-  sin = 0;
-  x = 0;
-  y = 0;
-}
-    
-void Reckoner::handleTickLeftForward()
-{
-    handleForward();
-    handleRight();
-}
-
-void Reckoner::handleTickLeftBackward()
-{
-    handleBackward();
-    handleLeft();   
-}
-
-void Reckoner::handleTickRightForward()
-{
-    handleForward();
-    handleLeft();   
-}
-
-void Reckoner::handleTickRightBackward()
-{
-    handleBackward();
-    handleRight();   
+    cosv = 1 << RECKONER_LOG_UNIT;
+    sinv = 0;
+    x = 0;
+    y = 0;
 }
 
 void Reckoner::handleForward()
 {
-    x += cos >> LOG_COS_TO_X_CONVERSION;
-    y += sin >> LOG_COS_TO_X_CONVERSION;
+    x += cosv;
+    y += sinv;
 }
 
 void Reckoner::handleBackward()
 {
-    x -= cos >> LOG_COS_TO_X_CONVERSION;
-    y -= sin >> LOG_COS_TO_X_CONVERSION;
-}
-
-void Reckoner::handleRight()
-{
-    // DA = 4790484
-    // 0.2% boost
-    handleTurnRadians(-4800065);
+    x -= cosv;
+    y -= sinv;
 }
 
-void Reckoner::handleLeft()
-{
-    handleTurnRadians(DA);
-}
-
-void Reckoner::handleTurnRadians(int32_t radians)
+void Reckoner::setTurnAngle(int32_t angle)
 {
-    int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE;
-    int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE;
-    dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
-    ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
-    cos += dc;
-    sin += ds;
-}
\ No newline at end of file
+    // 0x20000000 represents 45 degrees.
+    const double angleToRadiansFactor = 1.46291808e-9;  // pi/4 / 0x20000000
+    cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT));
+    sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT));
+}