Revision 40:6fa672be85ec, committed 2019-07-25

Comitter:

DavidEGrayson

Date:

Thu Jul 25 02:53:34 2019 +0000

Parent:

39:b19dfc5d4d4b

Child:

41:3ead1dd2cc3a

Commit message:

Add TurnSensor and L3G code but I am not happy with how the Gyro drifts a degree every few seconds or so.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/l3g.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -0,0 +1,95 @@
+// Code for reading from the L3GD20H gyro with I2C.
+
+#include <mbed.h>
+#include "l3g.h"
+#include "pc_serial.h"
+
+I2C i2c(p9, p10);
+
+int address = 0xD6;
+
+int32_t l3gInit()
+{
+    int whoami = l3gReadReg(0x0F);
+    if (whoami != 0xD7)
+    {    
+        pc.printf("l3g whoami error: %d\n", whoami);
+        return -1;  // wrong ID or no response
+    }
+    
+    // CTRL1: 800 Hz output data rate,
+    // low-pass filter cutoff 100 Hz
+    l3gWriteReg(0x20, 0xFF);
+
+    // CTRL4: 2000 dps full scale
+    l3gWriteReg(0x23, 0x20);
+
+    // CTRL5: High-pass filter disabled
+    l3gWriteReg(0x24, 0x00);
+
+    return 0;  // success
+}
+
+int32_t l3gReadReg(char reg)
+{
+    int result = i2c.write(address, &reg, 1); 
+    if (result != 0)
+    {
+        return -1;  // error
+    }
+
+    char data;
+    result = i2c.read(address, &data, 1);
+    if (result != 0)
+    {
+        return -2;  // error
+    }
+  
+    return data;
+}
+
+int32_t l3gWriteReg(char reg, char value)
+{
+    char data[2];
+    data[0] = reg;
+    data[1] = value;
+    int result = i2c.write(address, data, 2); 
+    if (result != 0)
+    {
+        return -1;  // error
+    }
+    
+    return 0;
+}
+
+int32_t l3gZAvailable()
+{
+    // Read STATUS
+    int32_t result = l3gReadReg(0x27);
+    if (result < 0)
+    {
+        return result;
+    }
+    
+    // Read the ZDA bit.
+    return result >> 2 & 1;
+}
+
+int32_t l3gZRead()
+{
+    char reg = 0x80 | 0x2C;  // OUT_Z_L, with slave address updating
+    int result = i2c.write(address, &reg, 1);
+    if (result != 0)
+    {
+        return -500001;
+    }
+    
+    char data[2];
+    result = i2c.read(address, data, 2);
+    if(result != 0)
+    {
+        return -500002;
+    }
+    
+    return (int16_t)(data[1] << 8 | data[0]);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/l3g.h	Thu Jul 25 02:53:34 2019 +0000
@@ -0,0 +1,8 @@
+#pragma once
+
+// Returns 0 for success, non-zero for error.
+int32_t l3gInit();
+int32_t l3gReadReg(char reg);
+int32_t l3gWriteReg(char reg, char value);
+int32_t l3gZRead();
+int32_t l3gZAvailable();
\ No newline at end of file

--- a/line_sensors.cpp	Thu Jul 25 02:11:25 2019 +0000
+++ b/line_sensors.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -1,14 +1,5 @@
 #include "line_sensors.h"
 
-
-/**
-AnalogIn lineSensorsAnalog[LINE_SENSOR_COUNT] = {
-    AnalogIn(p20), // brown wire, left-most sensor
-    AnalogIn(p19), // orange wire, middle sensor
-    AnalogIn(p17), // blue wire, right-most sensor
-};  // TODO: remove
-**/
-
 DigitalInOut lineSensorsDigital[LINE_SENSOR_COUNT] = {
     DigitalInOut(p18), // white wire, left-most sensor
     DigitalInOut(p19), // orange wire, middle sensor
@@ -46,25 +37,3 @@
         }
     }
 }
-
-
-/**
-uint16_t analogReadWithFilter(AnalogIn * input)
-{
-    uint16_t readings[3];
-    for(uint8_t i = 0; i < 3; i++)
-    {
-        readings[i] = input->read_u16();   
-    }
-    
-    if (readings[0] <= readings[1] && readings[0] >= readings[2])
-    {
-        return readings[0];
-    }
-    if (readings[1] <= readings[0] && readings[1] >= readings[2])
-    {
-        return readings[1];
-    }
-    return readings[2];
-}
-**/
\ No newline at end of file

--- a/logger.cpp	Thu Jul 25 02:11:25 2019 +0000
+++ b/logger.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -1,7 +1,6 @@
 #pragma once
 
 #include "logger.h"
-#include "main.h"
 #include "pc_serial.h"
 
 Logger::Logger()
@@ -14,7 +13,7 @@
     return entryIndex >= LOGGER_SIZE;
 }
 
-void Logger::log()
+void Logger::log(struct LogEntry * newEntry)
 {
     if (isFull())
     {
@@ -23,11 +22,7 @@
     
     LogEntry * entry = &entries[entryIndex];
     entryIndex++;
-    
-    //entry->cos = reckoner.cos >> 16;
-    //entry->sin = reckoner.sin >> 16;
-    entry->x = reckoner.x >> 16;
-    entry->y = reckoner.y >> 16;
+    *entry = *newEntry;    
 }
 
 void Logger::dump()
@@ -36,7 +31,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,%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	Thu Jul 25 02:11:25 2019 +0000
+++ b/logger.h	Thu Jul 25 02:53:34 2019 +0000
@@ -6,8 +6,7 @@
 
 struct LogEntry
 {
-    //int16_t cos;
-    //int16_t sin;
+    int16_t turnAngle;
     int16_t x;
     int16_t y;   
 };
@@ -16,13 +15,13 @@
 {
     public:
     Logger();
-    void log();
+    void log(struct LogEntry *);
     void dump();
     bool isFull();
+    int32_t getSize() { return entryIndex; }
     
     LogEntry entries[LOGGER_SIZE];
     
     // The index of the next entry to write to.
-    int32_t entryIndex;
+    uint32_t entryIndex;
 };
-

--- a/main.cpp	Thu Jul 25 02:11:25 2019 +0000
+++ b/main.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -12,11 +12,17 @@
 #include "reckoner.h"
 #include "buttons.h"
 #include "line_tracker.h"
+#include "l3g.h"
+#include "turn_sensor.h"
 
 Reckoner reckoner;
 LineTracker lineTracker;
+TurnSensor turnSensor;
 Logger logger;
-Pacer loggerPacer(50000);
+
+uint32_t totalEncoderCounts = 0;
+uint32_t nextLogEncoderCount = 0;
+const uint32_t logSpacing = 100;
 
 const int16_t drivingSpeed = 400;
 
@@ -32,6 +38,13 @@
 {
     pc.baud(115200);
     
+    if (l3gInit())
+    {
+        // Error initializing the gyro.
+        setLeds(0, 0, 1, 1);
+        while(1);
+    }
+    
     // Enable pull-ups on encoder pins and give them a chance to settle.
     encodersInit();
     motorsInit();
@@ -42,6 +55,8 @@
     //testEncoders();
     //testMotorSpeed();
     //testLineSensors();
+    //testL3g();
+    testTurnSensor();
     //testReckoner();
     //testButtons();
     //testDriveHome();
@@ -82,9 +97,15 @@
 
 void loggerService()
 {
-    if (loggerPacer.pace())
+    if (totalEncoderCounts > nextLogEncoderCount)
     {
-        logger.log();   
+        nextLogEncoderCount += logSpacing;
+        
+        struct LogEntry entry;
+        entry.turnAngle = turnSensor.getAngle() >> 16;
+        entry.x = reckoner.x >> 16;
+        entry.y = reckoner.y >> 16;
+        logger.log(&entry);
     }
 }
 
@@ -129,15 +150,19 @@
         {
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
             reckoner.handleTickLeftForward();
+            totalEncoderCounts++;
             break;
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
             reckoner.handleTickLeftBackward();
+            totalEncoderCounts--;
             break;
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
             reckoner.handleTickRightForward();
+            totalEncoderCounts++;
             break;
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
             reckoner.handleTickRightBackward();
+            totalEncoderCounts--;
             break;
         }
     }

--- a/test.cpp	Thu Jul 25 02:11:25 2019 +0000
+++ b/test.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -10,6 +10,8 @@
 #include "encoders.h"
 #include "pc_serial.h"
 #include "line_sensors.h"
+#include "l3g.h"
+#include "turn_sensor.h"
 #include "reckoner.h"
 #include "buttons.h"
 
@@ -306,6 +308,55 @@
     }
 }
 
+void testTurnSensor()
+{
+    pc.printf("Test turn sensor\r\n");
+    Pacer reportPacer(200000);
+    TurnSensor turnSensor;
+    turnSensor.start();
+    while(1)
+    {
+        turnSensor.update();
+        if (reportPacer.pace())
+        {
+            pc.printf("%d\r\n", turnSensor.getAngleDegrees());
+        }
+    }
+}
+
+void testL3g()
+{
+    Pacer reportPacer(750000);
+    Timer timer;
+    timer.start();
+    int32_t gz = 0;
+    bool reportedReading = false;
+    while(1)
+    {
+        int32_t result = l3gZAvailable();
+        if (result == 1)
+        {
+            gz = l3gZRead();
+            reportedReading = false;
+            if (gz > 100 || gz < -100)
+            {
+                pc.printf("%d, %d\r\n", timer.read_us(), gz); 
+                reportedReading = true;
+            }
+        }
+        else if (result != 0)
+        {
+            pc.printf("l3gZAvailable error: %d\n", result);
+        }
+
+        if (reportPacer.pace() && !reportedReading)
+        {
+            pc.printf("%d, %d\r\n", timer.read_us(), gz);
+            reportedReading = true;
+        }
+    }
+}
+
 void testLineSensors()
 {
     led1 = 1;
@@ -491,7 +542,6 @@
               determinant(), dotProduct());
        }
     }
-   
 }
 
 // with should be between 0 and 63

--- a/test.h	Thu Jul 25 02:11:25 2019 +0000
+++ b/test.h	Thu Jul 25 02:53:34 2019 +0000
@@ -4,6 +4,8 @@
 void __attribute__((noreturn)) testMotors();
 void __attribute__((noreturn)) testMotorSpeed();
 void __attribute__((noreturn)) testLineSensors();
+void __attribute__((noreturn)) testL3g();
+void __attribute__((noreturn)) testTurnSensor();
 void __attribute__((noreturn)) testReckoner();
 void __attribute__((noreturn)) testButtons();
 void __attribute__((noreturn)) testDriveHome();

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/turn_sensor.h	Thu Jul 25 02:53:34 2019 +0000
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <mbed.h>
+
+class TurnSensor
+{
+    // TODO: for production code, you would want a way to set the gyro offset
+
+    public:
+
+    void reset();
+    void start();
+    void update();
+    
+    int32_t getAngle()
+    {
+        return (int32_t)angleUnsigned;
+    }
+    
+    uint32_t getAngleUnsigned()
+    {
+        return angleUnsigned;
+    }
+    
+    int16_t getAngleDegrees()
+    {
+        return (((int32_t)angleUnsigned >> 16) * 360) >> 16;
+    }
+    
+    int16_t getRate()
+    {
+        return rate;
+    }
+    
+    private:
+
+    Timer timer;
+    uint32_t angleUnsigned;
+    int16_t rate;
+    uint16_t gyroLastUpdate;
+};
+
+
+// This constant represents a turn of 45 degrees.
+const int32_t turnAngle45 = 0x20000000;
+
+// This constant represents a turn of 90 degrees.
+const int32_t turnAngle90 = turnAngle45 * 2;
+
+// This constant represents a turn of approximately 1 degree.
+const int32_t turnAngle1 = (turnAngle45 + 22) / 45;
\ No newline at end of file