Revision 13:bba5b3abd13f, committed 2014-02-23

Comitter:

DavidEGrayson

Date:

Sun Feb 23 23:49:58 2014 +0000

Parent:

12:835a4d24ae3b

Child:

14:c8cca3687e64

Commit message:

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!

Changed in this revision

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

--- a/reckoner.h	Sun Feb 23 22:23:34 2014 +0000
+++ b/reckoner.h	Sun Feb 23 23:49:58 2014 +0000
@@ -1,7 +1,3 @@
-// Robot configuration:
-
-// General purpose code:
-
 class Reckoner
 {
     public:
@@ -9,28 +5,15 @@
     Reckoner();
     
     // Together, cos and sin form a vector with a magnitude close to 2^30 that indicates
-    // the current position of the robot.  By definition, when the reckoning starts,
-    // sin is 0 and cos is 2^30.
+    // the current position of the robot.
     // cos corresponds to the x component of the orientation vector.
     // sin corresponds to the y component of the orientation vector.
     int32_t cos, sin;
     
     // Together, x and y are a vector that points from the starting point to the
     // robot's current position.
-    // Units:
-    // * If the units are too big, precision is lost.
-    // * If they are too small, the integers wil overflow.
-    // * For convenience, they should be a power of 2 off from Df (Distance robot moves
-    //   forward per encoder tick), so we can just write x += cos >> some_constant.
-    // * Worst case for overflow: our encoders give us a Df of 0.25mm and our robot moves
-    //   5m away from the starting point.  Therefore, x and y might need to hold a value
-    //   20000 or -20000 without overflowing.
-    //   Overflow condition:              x = (1<<31)
-    //                20000*Df < max_dist_x = (1<<31) * U
-    //                                U > Df * (20000) / (1<<31) = Df / (1<<16)
-    // * Therefore the units we choose for x are Df / (1<<16).
-    // * If we wrote x += cos it would mean the units are Df / (1<<30).
-    //   Instead, we write x += cos >> 14 so the units are correct.
+    // The untis are Df / (1<<14), where Df is the distance the robot moves forward
+    // or backwards due to a single encoder tick.
     int32_t x, y;
 
     void handleTickLeftForward();

--- a/test.cpp	Sun Feb 23 22:23:34 2014 +0000
+++ b/test.cpp	Sun Feb 23 23:49:58 2014 +0000
@@ -15,14 +15,21 @@
 
 void testReckoner()
 {
+    Pacer reportPacer(100000);
     while(1)
     {
         updateReckonerFromEncoders();
-        led1 = (reckoner.x > 0);
-        led2 = (reckoner.y > 0);
-        led3 = (reckoner.cos > 0);
-        led4 = (reckoner.sin > 0);
+        led1 = (reckoner.cos > 0);
+        led2 = (reckoner.sin > 0);
+        led3 = (reckoner.x > 0);
+        led4 = (reckoner.y > 0);
         
+        if (reportPacer.pace())
+        {
+            pc.printf("%11d %11d %11d %11d | %8d %8d \n",
+              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              encoderLeft.getCount(), encoderRight.getCount());
+        }
     }
 }
 

--- a/test.h	Sun Feb 23 22:23:34 2014 +0000
+++ b/test.h	Sun Feb 23 23:49:58 2014 +0000
@@ -6,4 +6,7 @@
 void __attribute__((noreturn)) testReckoner();
 
 
+
+// These are actually defined in main.h:
+
 void updateReckonerFromEncoders();
\ No newline at end of file