Revision 42:96671b71aac5, committed 2019-07-27

Comitter:

DavidEGrayson

Date:

Sat Jul 27 20:58:46 2019 +0000

Parent:

41:3ead1dd2cc3a

Child:

43:0e985a58f174

Commit message:

Calibrate L3G using a buffer of 1000 zero-rate readings. Measured a drift of -1.850 degrees over 3 minutes (-0.01 degree per second).

Changed in this revision

--- a/l3g.cpp	Thu Jul 25 03:20:41 2019 +0000
+++ b/l3g.cpp	Sat Jul 27 20:58:46 2019 +0000
@@ -6,7 +6,7 @@
 
 I2C i2c(p9, p10);
 
-int address = 0xD6;
+const int address = 0xD6;
 
 int32_t l3gInit()
 {
@@ -92,4 +92,49 @@
     }
     
     return (int16_t)(data[1] << 8 | data[0]);
+}
+
+// The stuff below doesn't actually have anything to do with the L3G.
+
+#define L3G_CAL_COUNT_MAX 1000
+int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX];
+uint32_t l3gCalCount = 0;
+uint32_t l3gCalReplayIndex = 0;
+
+int32_t l3gCalibrate()
+{
+    int32_t reading = l3gZRead();
+    
+    if (l3gCalCount < L3G_CAL_COUNT_MAX)
+    {
+        int8_t c;
+        if (reading > 127) { c = 127; }
+        else if (reading < -128) { c = -128; }
+        else { c = reading; }
+        l3gCalBuffer[l3gCalCount++] = c;
+    }
+    
+    return reading;
+}
+
+bool l3gCalibrateDone()
+{
+    return l3gCalCount >= L3G_CAL_COUNT_MAX;
+}
+
+void l3gCalibrateReset()
+{
+    l3gCalCount = 0;
+}
+
+int32_t l3gZReadCalibrated()
+{
+    int8_t zeroRate = 0;
+    if (l3gCalCount)
+    {
+      if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; }
+      zeroRate = l3gCalBuffer[l3gCalReplayIndex++];
+    }
+    
+    return l3gZRead() - zeroRate;
 }
\ No newline at end of file

--- a/l3g.h	Thu Jul 25 03:20:41 2019 +0000
+++ b/l3g.h	Sat Jul 27 20:58:46 2019 +0000
@@ -5,4 +5,14 @@
 int32_t l3gReadReg(char reg);
 int32_t l3gWriteReg(char reg, char value);
 int32_t l3gZRead();
-int32_t l3gZAvailable();
\ No newline at end of file
+int32_t l3gZAvailable();
+
+// Takes one reading and record it for the purposes of zero-rate calibrations.
+// Returns the reading just in case you want it for something.
+int32_t l3gCalibrate();
+
+bool l3gCalibrateDone();
+
+void l3gCalibrateReset();
+
+int32_t l3gZReadCalibrated();

--- a/main.cpp	Thu Jul 25 03:20:41 2019 +0000
+++ b/main.cpp	Sat Jul 27 20:58:46 2019 +0000
@@ -58,7 +58,7 @@
     //testMotorSpeed();
     //testLineSensors();
     //testL3g();
-    //testL3gAndCalibrate();
+    //testL3gAndShowAverage();
     testTurnSensor();
     //testReckoner();
     //testButtons();
@@ -72,7 +72,8 @@
     //testCloseness();
     //testLogger();
 
-    loadCalibration();
+    loadLineCalibration();
+    doGyroCalibration();
 
     doDeadReckoning();
 }
@@ -126,8 +127,17 @@
     }   
 }
 
+void doGyroCalibration()
+{
+    wait_ms(1000);  // Time for the robot to stop moving.
+    while (!l3gCalibrateDone())
+    {
+        l3gCalibrate();
+        wait_ms(2);
+    }
+}
 
-void loadCalibration()
+void loadLineCalibration()
 {
     /** QTR-3RC **/
     lineTracker.calibratedMinimum[0] =  100;
@@ -136,15 +146,6 @@
     lineTracker.calibratedMaximum[0] =  792;
     lineTracker.calibratedMaximum[1] =  807;
     lineTracker.calibratedMaximum[2] = 1000;    
-    
-    /** QTR-3A
-    lineTracker.calibratedMinimum[0] = 34872;
-    lineTracker.calibratedMinimum[1] = 29335;
-    lineTracker.calibratedMinimum[2] = 23845;
-    lineTracker.calibratedMaximum[0] = 59726;
-    lineTracker.calibratedMaximum[1] = 60110;
-    lineTracker.calibratedMaximum[2] = 58446;
-    **/
 }
 
 void updateReckonerFromEncoders()

--- a/main.h	Thu Jul 25 03:20:41 2019 +0000
+++ b/main.h	Sat Jul 27 20:58:46 2019 +0000
@@ -3,8 +3,10 @@
 #include "reckoner.h"
 #include "line_tracker.h"
 #include "logger.h"
+#include "turn_sensor.h"
 
-void loadCalibration();
+void loadLineCalibration();
+void doGyroCalibration();
 
 void waitForSignalToStart();
 void findLineAndCalibrate(); void findLine();  // two alternatives
@@ -24,4 +26,5 @@
 
 extern Reckoner reckoner;
 extern LineTracker lineTracker;
-extern Logger logger;
\ No newline at end of file
+extern TurnSensor turnSensor;
+extern Logger logger;

--- a/test.cpp	Thu Jul 25 03:20:41 2019 +0000
+++ b/test.cpp	Sat Jul 27 20:58:46 2019 +0000
@@ -208,7 +208,7 @@
     
     Pacer reportPacer(200000);
     
-    loadCalibration();
+    loadLineCalibration();
     uint16_t loopCount = 0;
     while(1)
     {
@@ -290,6 +290,8 @@
 
 void testReckoner()
 {
+    doGyroCalibration();
+    led1 = 1;
     Pacer reportPacer(100000);
     while(1)
     {
@@ -311,26 +313,32 @@
 void testTurnSensor()
 {
     pc.printf("Test turn sensor\r\n");
-    Pacer reportPacer(200000);
-    TurnSensor turnSensor;
+    led1 = 1;
+    doGyroCalibration();
+    //Pacer reportPacer(200000);  // 0.2 s
+    Pacer reportPacer(10000000);  // 10 s
+    Timer timer;
+    led2 = 1;
+    timer.start();
     turnSensor.start();
     while(1)
     {
         turnSensor.update();
         if (reportPacer.pace())
         {
-            pc.printf("%d\r\n", turnSensor.getAngleDegrees());
+            pc.printf("%u, %d, %d\r\n",
+              timer.read_ms(),
+              turnSensor.getAngleDegrees(),
+              turnSensor.getAngleMillidegrees());
         }
     }
 }
 
-void testL3gAndCalibrate()
+void testL3gAndShowAverage()
 {
     wait_ms(2000);
     Pacer reportPacer(750000);
     Pacer readingPacer(2000);
-    Timer timer;
-    timer.start();
     int32_t total = 0;
     int32_t readingCount = 0;
     while(1)
@@ -384,7 +392,7 @@
             reportedReading = false;
             if (gz > 100 || gz < -100)
             {
-                pc.printf("%d, %d\r\n", timer.read_us(), gz); 
+                pc.printf("%u, %d\r\n", timer.read_us(), gz); 
                 reportedReading = true;
             }
         }
@@ -395,7 +403,7 @@
 
         if (reportPacer.pace() && !reportedReading)
         {
-            pc.printf("%d, %d\r\n", timer.read_us(), gz);
+            pc.printf("%u, %d\r\n", timer.read_us(), gz);
             reportedReading = true;
         }
     }

--- a/test.h	Thu Jul 25 03:20:41 2019 +0000
+++ b/test.h	Sat Jul 27 20:58:46 2019 +0000
@@ -5,7 +5,7 @@
 void __attribute__((noreturn)) testMotorSpeed();
 void __attribute__((noreturn)) testLineSensors();
 void __attribute__((noreturn)) testL3g();
-void __attribute__((noreturn)) testL3gAndCalibrate();
+void __attribute__((noreturn)) testL3gAndShowAverage();
 void __attribute__((noreturn)) testTurnSensor();
 void __attribute__((noreturn)) testReckoner();
 void __attribute__((noreturn)) testButtons();

--- a/turn_sensor.cpp	Thu Jul 25 03:20:41 2019 +0000
+++ b/turn_sensor.cpp	Sat Jul 27 20:58:46 2019 +0000
@@ -18,19 +18,12 @@
 {
     if (l3gZAvailable() == 1)
     {
-        int32_t gz = l3gZRead();
+        int32_t gz = l3gZReadCalibrated();
         if (gz < -500000)
         {
             // error
             return;
-        }
-        
-        // The gyro zero rate on my robot, measured by testL3gAndCalibrate() on
-        // 2019-07-24 is about -6.5.  So let's add 6 half the time and add 7
-        // the other half of the time.  This is a big hack.
-        static uint8_t updateCount = 0;
-        updateCount++;
-        gz = gz + 6 + (updateCount & 1);
+        }        
 
         // The gyro on this robot is mounted upside down; account for that here so that
         // we can have counter-clockwise be a positive rotation.

--- a/turn_sensor.h	Thu Jul 25 03:20:41 2019 +0000
+++ b/turn_sensor.h	Sat Jul 27 20:58:46 2019 +0000
@@ -27,6 +27,11 @@
         return (((int32_t)angleUnsigned >> 16) * 360) >> 16;
     }
     
+    int32_t getAngleMillidegrees()
+    {
+        return ((int64_t)(int32_t)angleUnsigned * 360000) >> 32;
+    }
+    
     int16_t getRate()
     {
         return rate;