Revision 41:3ead1dd2cc3a, committed 2019-07-25

Comitter:

DavidEGrayson

Date:

Thu Jul 25 03:20:41 2019 +0000

Parent:

40:6fa672be85ec

Child:

42:96671b71aac5

Commit message:

Gyro: Add a hacky offset of +6.5 in TurnSensor.cpp. The turn sensor drifts much more slowly now, like maybe 1 degree per minute.

Changed in this revision

--- a/main.cpp	Thu Jul 25 02:53:34 2019 +0000
+++ b/main.cpp	Thu Jul 25 03:20:41 2019 +0000
@@ -15,6 +15,8 @@
 #include "l3g.h"
 #include "turn_sensor.h"
 
+void doDeadReckoning();
+
 Reckoner reckoner;
 LineTracker lineTracker;
 TurnSensor turnSensor;
@@ -56,6 +58,7 @@
     //testMotorSpeed();
     //testLineSensors();
     //testL3g();
+    //testL3gAndCalibrate();
     testTurnSensor();
     //testReckoner();
     //testButtons();
@@ -68,17 +71,20 @@
     //testTurnInPlace();
     //testCloseness();
     //testLogger();
-    
+
+    loadCalibration();
 
-    // Real routines for the contest.
-    loadCalibration();
-    
+    doDeadReckoning();
+}
+
+void doDeadReckoning()
+{
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
     
     setLeds(0, 1, 0, 0);    
     findLine();
-    
+
     //setLeds(1, 1, 0, 0);
     //turnRightToFindLine();
     

--- a/test.cpp	Thu Jul 25 02:53:34 2019 +0000
+++ b/test.cpp	Thu Jul 25 03:20:41 2019 +0000
@@ -324,6 +324,50 @@
     }
 }
 
+void testL3gAndCalibrate()
+{
+    wait_ms(2000);
+    Pacer reportPacer(750000);
+    Pacer readingPacer(2000);
+    Timer timer;
+    timer.start();
+    int32_t total = 0;
+    int32_t readingCount = 0;
+    while(1)
+    {
+        if (readingPacer.pace())
+        {
+          int32_t result = l3gZAvailable();
+          if (result == 1)
+          {
+              int32_t gz = l3gZRead();
+              if (gz < -500000)
+              {
+                  pc.printf("l3gZRead error: %d\n", gz);
+              }
+              else
+              {
+                  total += gz;
+                  readingCount += 1;
+              }
+          }
+          else if (result != 0)
+          {
+              pc.printf("l3gZAvailable error: %d\n", result);
+          }
+        }
+
+        if (reportPacer.pace())
+        {
+            float average = (float)total / readingCount;
+            pc.printf("%d, %d, %f\r\n", total, readingCount, average);
+        }
+    }
+    
+    // Gyro calibration results get hardcoded into TurnSensor::update()
+    // for now until we figure out something better.
+}
+
 void testL3g()
 {
     Pacer reportPacer(750000);

--- a/test.h	Thu Jul 25 02:53:34 2019 +0000
+++ b/test.h	Thu Jul 25 03:20:41 2019 +0000
@@ -5,6 +5,7 @@
 void __attribute__((noreturn)) testMotorSpeed();
 void __attribute__((noreturn)) testLineSensors();
 void __attribute__((noreturn)) testL3g();
+void __attribute__((noreturn)) testL3gAndCalibrate();
 void __attribute__((noreturn)) testTurnSensor();
 void __attribute__((noreturn)) testReckoner();
 void __attribute__((noreturn)) testButtons();

--- a/turn_sensor.cpp	Thu Jul 25 02:53:34 2019 +0000
+++ b/turn_sensor.cpp	Thu Jul 25 03:20:41 2019 +0000
@@ -25,10 +25,17 @@
             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.
-        gz = -gz;
-        
+        gz = -gz;        
+       
         rate = gz;
         
         // First figure out how much time has passed since the last update (dt)
@@ -49,11 +56,6 @@
         //
         // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
         // = 14680064/17578125 unit/(digit*us)
-        //const float factor = (float)14680064 / 17578125;
-        
-        // Fudge factor to account for the fact that the gyro might be mounted
-        // at a bad angle or it might be more or less sensitive than expected.
-        //const float fudge = 0.98809906722;
         
         angleUnsigned += (int64_t)d * 14680064 / 17578125;
     }