Revision 12:835a4d24ae3b, committed 2014-02-23

Comitter:

DavidEGrayson

Date:

Sun Feb 23 22:23:34 2014 +0000

Parent:

11:bd14d512340a

Child:

13:bba5b3abd13f

Commit message:

Made the Reckoner class and wrote a routine to help test it.;

Changed in this revision

--- a/encoders.cpp	Sat Feb 22 04:53:35 2014 +0000
+++ b/encoders.cpp	Sun Feb 23 22:23:34 2014 +0000
@@ -6,8 +6,8 @@
               encoderPinRightB = p29;  // Gray wire
 
 PololuEncoderBuffer encoderBuffer;
-PololuEncoder encoderLeft(encoderPinLeftA, encoderPinLeftB, &encoderBuffer, ENCODER1);
-PololuEncoder encoderRight(encoderPinRightA, encoderPinRightB, &encoderBuffer, ENCODER2);
+PololuEncoder encoderLeft(encoderPinLeftA, encoderPinLeftB, &encoderBuffer, ENCODER_LEFT);
+PololuEncoder encoderRight(encoderPinRightA, encoderPinRightB, &encoderBuffer, ENCODER_RIGHT);
 
 void encodersInit()
 {

--- a/encoders.h	Sat Feb 22 04:53:35 2014 +0000
+++ b/encoders.h	Sun Feb 23 22:23:34 2014 +0000
@@ -2,8 +2,8 @@
 
 #include <PololuEncoder.h>
 
-#define ENCODER1 0x00
-#define ENCODER2 0x01
+#define ENCODER_LEFT 0x00
+#define ENCODER_RIGHT 0x01
 
 extern PololuEncoderBuffer encoderBuffer;
 extern PololuEncoder encoderLeft, encoderRight;

--- a/main.cpp	Sat Feb 22 04:53:35 2014 +0000
+++ b/main.cpp	Sun Feb 23 22:23:34 2014 +0000
@@ -6,6 +6,7 @@
 #include "leds.h"
 #include "pc_serial.h"
 #include "test.h"
+#include "reckoner.h"
 
 int __attribute__((noreturn)) main()
 {
@@ -18,10 +19,35 @@
     // Test routines
     //testMotors();
     //testEncoders();
-    testLineSensors();
+    //testLineSensors();
+    testReckoner();
 
     while(1)
     {
 
     }
 }
+
+void updateReckonerFromEncoders()
+{
+    while(encoderBuffer.hasEvents())
+    {
+        PololuEncoderEvent event = encoderBuffer.readEvent();
+        switch(event)
+        {
+            case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
+                reckoner.handleTickLeftForward();
+                break;
+            case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
+                reckoner.handleTickLeftBackward();
+                break;
+            case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
+                reckoner.handleTickRightForward();
+                break;
+            case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
+                reckoner.handleTickRightBackward();
+                break;
+                   
+        }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/reckoner.cpp	Sun Feb 23 22:23:34 2014 +0000
@@ -0,0 +1,77 @@
+#include <mbed.h>
+#include "reckoner.h"
+
+/**
+W: Wheel-to-wheel distance:              150 mm  // TODO: correct this
+G: Gear ratio factor:                    30
+T: Encoder ticks per backshaft rotation: 12       (three-toothed encoder wheel)
+R: Wheel radius:                         70 mm
+
+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
+**/
+
+#define LOG_UNIT_MAGNITUDE 30
+
+#define DA 8745487  // 0.00814486984 * 0x40000000
+
+#define LOG_COS_TO_X_CONVERSION  14    // 30 - 16
+
+Reckoner reckoner;
+
+Reckoner::Reckoner()
+{
+  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();   
+}
+
+void Reckoner::handleForward()
+{
+    x += cos >> LOG_COS_TO_X_CONVERSION;
+    y += sin >> LOG_COS_TO_X_CONVERSION;
+}
+
+void Reckoner::handleBackward()
+{
+    x -= cos >> LOG_COS_TO_X_CONVERSION;
+    y -= sin >> LOG_COS_TO_X_CONVERSION;
+}
+
+void Reckoner::handleRight()
+{
+    cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
+    sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+}
+
+void Reckoner::handleLeft()
+{
+    cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
+    sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/reckoner.h	Sun Feb 23 22:23:34 2014 +0000
@@ -0,0 +1,46 @@
+// Robot configuration:
+
+// General purpose code:
+
+class Reckoner
+{
+    public:
+    
+    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.
+    // 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.
+    int32_t x, y;
+
+    void handleTickLeftForward();
+    void handleTickLeftBackward();
+    void handleTickRightForward();
+    void handleTickRightBackward();
+    void handleForward();
+    void handleBackward();
+    void handleRight();
+    void handleLeft();
+};
+
+extern Reckoner reckoner;
\ No newline at end of file

--- a/test.cpp	Sat Feb 22 04:53:35 2014 +0000
+++ b/test.cpp	Sun Feb 23 22:23:34 2014 +0000
@@ -9,9 +9,23 @@
 #include "encoders.h"
 #include "pc_serial.h"
 #include "line_sensors.h"
+#include "reckoner.h"
 
 void printBar(const char * name, uint16_t adcResult);
 
+void testReckoner()
+{
+    while(1)
+    {
+        updateReckonerFromEncoders();
+        led1 = (reckoner.x > 0);
+        led2 = (reckoner.y > 0);
+        led3 = (reckoner.cos > 0);
+        led4 = (reckoner.sin > 0);
+        
+    }
+}
+
 void testLineSensors()
 {
     led1 = 1;

--- a/test.h	Sat Feb 22 04:53:35 2014 +0000
+++ b/test.h	Sun Feb 23 22:23:34 2014 +0000
@@ -2,4 +2,8 @@
 
 void __attribute__((noreturn)) testEncoders();
 void __attribute__((noreturn)) testMotors();
-void __attribute__((noreturn)) testLineSensors();
\ No newline at end of file
+void __attribute__((noreturn)) testLineSensors();
+void __attribute__((noreturn)) testReckoner();
+
+
+void updateReckonerFromEncoders();
\ No newline at end of file