Diff: reckoner.cpp

Revision:

13:bba5b3abd13f

Parent:

12:835a4d24ae3b

Child:

14:c8cca3687e64

--- a/reckoner.cpp	Sun Feb 23 22:23:34 2014 +0000
+++ b/reckoner.cpp	Sun Feb 23 23:49:58 2014 +0000
@@ -2,21 +2,88 @@
 #include "reckoner.h"
 
 /**
-W: Wheel-to-wheel distance:              150 mm  // TODO: correct this
+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:                         70 mm
+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*70/(30*12) / 150 = 0.00814486984
+dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394
 **/
 
-#define LOG_UNIT_MAGNITUDE 30
+#define DA 4718788  // 0.00439471394 * 0x40000000
 
-#define DA 8745487  // 0.00814486984 * 0x40000000
-
-#define LOG_COS_TO_X_CONVERSION  14    // 30 - 16
+#define LOG_COS_TO_X_CONVERSION  16    // 30 - 14
 
 Reckoner reckoner;