Diff: turn_sensor.cpp

Revision:

40:6fa672be85ec

Child:

41:3ead1dd2cc3a

diff -r b19dfc5d4d4b -r 6fa672be85ec turn_sensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/turn_sensor.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -0,0 +1,60 @@
+#include "turn_sensor.h"
+#include "l3g.h"
+
+void TurnSensor::reset()
+{
+    angleUnsigned = 0;
+}
+
+void TurnSensor::start()
+{
+    timer.start(); 
+    rate = 0;  
+    angleUnsigned = 0;
+    gyroLastUpdate = timer.read_us();
+}
+
+void TurnSensor::update()
+{
+    if (l3gZAvailable() == 1)
+    {
+        int32_t gz = l3gZRead();
+        if (gz < -500000)
+        {
+            // 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)
+        uint16_t m = timer.read_us();
+        uint16_t dt = m - gyroLastUpdate;
+        gyroLastUpdate = m;
+
+        // Multiply dt by turnRate in order to get an estimation of how
+        // much the robot has turned since the last update.
+        // (angular change = angular velocity * time)
+        int32_t d = (int32_t)rate * dt;
+
+        // The units of d are gyro digits times microseconds.  We need
+        // to convert those to the units of turnAngle, where 2^29 units
+        // represents 45 degrees.  The conversion from gyro digits to
+        // degrees per second (dps) is determined by the sensitivity of
+        // the gyro: 0.07 degrees per second per digit.
+        //
+        // (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;
+    }
+}
\ No newline at end of file