Revision 46:f11cb4f93aac, committed 2015-04-15

Comitter:

DavidEGrayson

Date:

Wed Apr 15 21:19:22 2015 +0000

Parent:

45:e16e74bbbf8c

Child:

47:cb5c1504c24d

Commit message:

got dead reckoning working nicely with the gyro;

Changed in this revision

--- a/logger.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/logger.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -24,8 +24,7 @@
     LogEntry * entry = &entries[entryIndex];
     entryIndex++;
     
-    //entry->cos = reckoner.cos >> 16;
-    //entry->sin = reckoner.sin >> 16;
+    entry->turnAngle = 0;
     entry->x = reckoner.x >> 16;
     entry->y = reckoner.y >> 16;
 }
@@ -36,7 +35,7 @@
     for(int32_t i = 0; i < entryIndex; i++)
     {
         LogEntry * entry = &entries[i];
-        pc.printf("%d,%d\r\n", entry->x, entry->y);
+        pc.printf("%d,%d\r\n", entry->turnAngle, entry->x, entry->y);
     }
     pc.printf("Log dump end\r\n");
 }
\ No newline at end of file

--- a/logger.h	Wed Apr 15 19:19:19 2015 +0000
+++ b/logger.h	Wed Apr 15 21:19:22 2015 +0000
@@ -6,8 +6,7 @@
 
 struct LogEntry
 {
-    //int16_t cos;
-    //int16_t sin;
+    int16_t turnAngle;
     int16_t x;
     int16_t y;   
 };

--- a/main.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/main.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -60,7 +60,8 @@
     //testCloseness();
     //testLogger();
     //testL3g();
-    testTurnSensor();
+    //testTurnSensor();
+    //testReckoningWithGyro();
 
     // Real routines for the contest.
     loadCalibration();
@@ -138,6 +139,32 @@
     }
 }
 
+void updateReckoner(TurnSensor & turnSensor)
+{
+    if (!encoderBuffer.hasEvents())
+    {
+        return;
+    }
+    
+    reckoner.setTurnAngle(turnSensor.getAngle());
+    
+    while(encoderBuffer.hasEvents())
+    {
+        PololuEncoderEvent event = encoderBuffer.readEvent();
+        switch(event)
+        {
+        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
+        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
+            reckoner.handleForward();
+            break;
+        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
+        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
+            reckoner.handleBackward();
+            break;
+        }
+    }
+}
+
 float magnitude()
 {
     return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);   
@@ -145,8 +172,8 @@
 
 float dotProduct()
 {
-    float s = (float)reckoner.sin / (1 << 30);
-    float c = (float)reckoner.cos / (1 << 30);
+    float s = (float)reckoner.sinv / (1 << 30);
+    float c = (float)reckoner.cosv / (1 << 30);
     float magn = magnitude();
     if (magn == 0){ return 0; }    
     return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
@@ -157,8 +184,8 @@
 float determinant()
 {
     // TODO: get rid of the magic numbers here (i.e. 30)
-    float s = (float)reckoner.sin / (1 << 30);
-    float c = (float)reckoner.cos / (1 << 30);
+    float s = (float)reckoner.sinv / (1 << 30);
+    float c = (float)reckoner.cosv / (1 << 30);
     return (reckoner.x * s - reckoner.y * c) / magnitude();
 }
 
@@ -241,10 +268,13 @@
     uint32_t loopCount = 0;
     Timer timer;
     timer.start();
+    TurnSensor turnSensor;
+    turnSensor.start();
     while(1)
     {
         loopCount += 1;
-        updateReckonerFromEncoders();
+        turnSensor.update();
+        updateReckoner(turnSensor);
         loggerService();
         
         if ((loopCount % 256) == 0)

--- a/main.h	Wed Apr 15 19:19:19 2015 +0000
+++ b/main.h	Wed Apr 15 21:19:22 2015 +0000
@@ -3,6 +3,7 @@
 #include "reckoner.h"
 #include "line_tracker.h"
 #include "logger.h"
+#include "turn_sensor.h"
 
 void loadCalibration();
 
@@ -13,11 +14,13 @@
 
 void updateMotorsToFollowLine();
 void updateReckonerFromEncoders();
+void updateReckoner(TurnSensor &);
 void setLeds(bool v1, bool v2, bool v3, bool v4);
 float determinant();
 float dotProduct();
 float magnitude();
 void loggerService();
+void waitForSignalToStart();
 
 extern Reckoner reckoner;
 extern LineTracker lineTracker;

--- a/reckoner.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/reckoner.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include "reckoner.h"
+#include "math.h"
 
 /**
 First, we define two int32_t variables cos and sin that make a unit vector.
@@ -7,7 +8,7 @@
 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].
+The starting value of the [cos, sin] will be [1 << 30, 0].
 To record this choice, we define LOG_UNIT_MAGNITUDE:
 **/
 #define LOG_UNIT_MAGNITUDE 30
@@ -109,8 +110,8 @@
 
 void Reckoner::reset()
 {
-  cos = 1 << LOG_UNIT_MAGNITUDE;
-  sin = 0;
+  cosv = 1 << LOG_UNIT_MAGNITUDE;
+  sinv = 0;
   x = 0;
   y = 0;
 }
@@ -141,14 +142,14 @@
 
 void Reckoner::handleForward()
 {
-    x += cos >> LOG_COS_TO_X_CONVERSION;
-    y += sin >> LOG_COS_TO_X_CONVERSION;
+    x += cosv >> LOG_COS_TO_X_CONVERSION;
+    y += sinv >> LOG_COS_TO_X_CONVERSION;
 }
 
 void Reckoner::handleBackward()
 {
-    x -= cos >> LOG_COS_TO_X_CONVERSION;
-    y -= sin >> LOG_COS_TO_X_CONVERSION;
+    x -= cosv >> LOG_COS_TO_X_CONVERSION;
+    y -= sinv >> LOG_COS_TO_X_CONVERSION;
 }
 
 void Reckoner::handleRight()
@@ -165,10 +166,18 @@
 
 void Reckoner::handleTurnRadians(int32_t radians)
 {
-    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;
+    int32_t dc = -((int64_t)sinv * radians) >> LOG_UNIT_MAGNITUDE;
+    int32_t ds = ((int64_t)cosv * radians) >> LOG_UNIT_MAGNITUDE;
+    dc = -((int64_t)(sinv+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
+    ds = ((int64_t)(cosv+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
+    cosv += dc;
+    sinv += ds;
+}
+
+// For angle, 0x20000000 represents 45 degrees.
+void Reckoner::setTurnAngle(int32_t angle)
+{
+    double angleToRadiansFactor = 1.46291808e-9;  // pi/4 / 0x20000000
+    cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
+    sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE));
 }
\ No newline at end of file

--- a/reckoner.h	Wed Apr 15 19:19:19 2015 +0000
+++ b/reckoner.h	Wed Apr 15 21:19:22 2015 +0000
@@ -10,7 +10,7 @@
     // 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;
+    int32_t cosv, sinv;
     
     // Together, x and y are a vector that points from the starting point to the
     // robot's current position.
@@ -29,4 +29,5 @@
     void handleRight();
     void handleLeft();
     void handleTurnRadians(int32_t radians);
+    void setTurnAngle(int32_t angle);
 };

--- a/test.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/test.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -47,8 +47,8 @@
 
 void showOrientationWithLeds34()
 {
-    led3 = reckoner.cos > 0;
-    led4 = reckoner.sin > 0;
+    led3 = reckoner.cosv > 0;
+    led4 = reckoner.sinv > 0;
 }
 
 void testTurnInPlace()
@@ -78,7 +78,7 @@
         if (motorUpdatePacer.pace())
         {
             int16_t rotationSpeed;
-            float s = (float)reckoner.sin / (1 << 30);
+            float s = (float)reckoner.sinv / (1 << 30);
             integral += s;
             rotationSpeed = -(s * 2400 + integral * 20);
             
@@ -272,7 +272,7 @@
         if (reportPacer.pace())
         {
             pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               encoderLeft.getCount(), encoderRight.getCount(), determinant());
         }
     }
@@ -439,7 +439,7 @@
         if(reportPacer.pace())
         {
             pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               determinant(), dotProduct());
        }
     }
@@ -507,4 +507,32 @@
             pc.printf("%d\r\n", turnSensor.getAngleDegrees());
         }
     }
+}
+
+void testReckoningWithGyro()
+{   
+    setLeds(0, 0, 0, 1);
+    waitForSignalToStart();
+    setLeds(1, 0, 0, 1);
+
+    loadCalibration();
+    Timer timer;
+    timer.start();
+    TurnSensor turnSensor;
+    turnSensor.start();
+    while(1)
+    {
+        turnSensor.update();
+        updateReckoner(turnSensor);
+        loggerService();
+        
+        if (button1DefinitelyPressed())
+        {
+            break;
+        }
+    }
+    motorsSpeedSet(0, 0);
+    
+    setLeds(1, 1, 1, 1);
+    loggerReportLoop();
 }
\ No newline at end of file

--- a/test.h	Wed Apr 15 19:19:19 2015 +0000
+++ b/test.h	Wed Apr 15 21:19:22 2015 +0000
@@ -15,4 +15,5 @@
 void __attribute__((noreturn)) testCloseness();
 void __attribute__((noreturn)) testLogger();
 void __attribute__((noreturn)) testL3g();
-void __attribute__((noreturn)) testTurnSensor();
\ No newline at end of file
+void __attribute__((noreturn)) testTurnSensor();
+void __attribute__((noreturn)) testReckoningWithGyro();
\ No newline at end of file

--- a/turn_sensor.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/turn_sensor.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -19,6 +19,11 @@
             // error
             return;
         }
+        
+        // The gyro on this robot is mounted upside down; account for that here so that
+        // we can have counter-clockwise be a positive rotation.
+        gz = -gz;
+        
         rate = gz;
         
         // First figure out how much time has passed since the last update (dt)