Diff: Sensors/Sensors.cpp

Revision:

12:5dfa1ab47838

Parent:

2:fbc6e3cf3ed8

--- a/Sensors/Sensors.cpp	Thu Nov 29 16:35:31 2018 +0000
+++ b/Sensors/Sensors.cpp	Thu Nov 29 16:50:46 2018 +0000
@@ -28,48 +28,68 @@
 
 */
 
+#include <stdio.h>
+#include "globals.h"
+#include "Config.h"
+#include "devices.h"
 #include "Sensors.h"
+#include "util.h"
 #include "debug.h"
-#include "stdio.h"
-
-#define GYRO_SCALE 14.49787 // TODO: is the sign right here?? yes, see g_sign
 
 #define VFF 50.0 // voltage filter factor
 
 ///////////////////// MAGNETOMETER CALIBRATION
 
 Sensors::Sensors():
-    gps(p26, p25),
+    gTemp(0),
+    leftRanger(0),
+    rightRanger(0),
+    centerRanger(0),
+    voltage(0),
+    current(0),
+    leftTotal(0),
+    rightTotal(0),
+    leftCount(0),
+    rightCount(0),
+    lrEncDistance(0.0),
+    rrEncDistance(0.0),
+    lrEncSpeed(0.0),
+    rrEncSpeed(0.0),
+    encDistance(0.0),
+    encSpeed(0.0),
+    gps(GPSTX, GPSRX),
     _voltage(p19),               // Voltage from sensor board
     _current(p20),               // Current from sensor board
-    _left(p30),                  // left wheel encoder
-    _right(p29),                 // Right wheel encoder
-    _gyro(p28, p27),             // MinIMU-9 gyro
-    _compass(p28, p27),          // MinIMU-9 compass/accelerometer
-    _rangers(p28, p27),          // Arduino ADC to I2C
-    _cam(p28, p27)
+    _left(ENCALEFT),                  // left wheel encoder
+    _right(ENCARIGHT),                 // Right wheel encoder
+    _gyro(I2CSDA, I2CSCL),       // MinIMU-9 gyro
+    _compass(I2CSDA, I2CSCL),    // MinIMU-9 compass/accelerometer
+    _rangers(I2CSDA, I2CSCL),    // Arduino ADC to I2C
+    _cam(I2CSDA, I2CSCL)
+,   _tireCirc(0)
+,   _encStripes(0)
 {
     for (int i=0; i < 3; i++) {
         m_offset[i] = 0;
         m_scale[i] = 1;
     }
 
-    // TODO: parameterize
-    g_scale[0] = 1;
-    g_scale[1] = 1;
-    g_scale[2] = GYRO_SCALE;
+    // TODO 2 parameterize scale and sign for mag, accel, gyro
+    g_scale[_x_] = 1;
+    g_scale[_y_] = 1;
+    g_scale[_z_] = 1;
 
-    g_sign[0] = 1;
-    g_sign[1] = -1;
-    g_sign[2] = -1;
+    g_sign[_x_] = 1;
+    g_sign[_y_] = -1;
+    g_sign[_z_] = -1;
 
-    a_sign[0] = -1;
-    a_sign[1] = 1;
-    a_sign[2] = 1;
+    a_sign[_x_] = -1;
+    a_sign[_y_] = 1;
+    a_sign[_z_] = 1;
 
-    m_sign[0] = 1;
-    m_sign[1] = -1;
-    m_sign[2] = -1;
+    m_sign[_x_] = 1;
+    m_sign[_y_] = -1;
+    m_sign[_z_] = -1;
 
     // upside down mounting
     //g_sign[3] = {1,1,1};        
@@ -77,6 +97,11 @@
     //m_sign[3] = {1,1,1}; 
 }
 
+void Sensors::setGyroScale(float scale) {
+    g_scale[_z_] = scale;
+}
+
+
 /* Compass_Calibrate
  *
  * Set the offset and scale calibration for the compass
@@ -91,6 +116,12 @@
     return;
 }
 
+void Sensors::configureEncoders(float tireCirc, int encStripes)
+{
+    _tireCirc = tireCirc;
+    _encStripes = encStripes;
+}
+
 
 void Sensors::Read_Encoders()
 {
@@ -99,17 +130,16 @@
     rightCount = _right.read();
     leftTotal += leftCount;
     rightTotal += rightCount;
-            
-    // TODO--> sanity check on encoders; if difference between them
-    //  is huge, what do we do?  Slipping wheel?  Skidding wheel?
+
+    float ticksPerDist = _tireCirc / _encStripes;
+
+    // TODO 3 sanity check on encoders; if difference between them
+    //  is huge, what do we do?  Slipping wheel?  Skidding wheel?  Broken encoder?
     //  front encoders would be ideal as additional sanity check
     
-    // TODO: move this into scheduler??
-    
-    // TODO: how do we track distance, should we only check distance everytime we do a nav/pos update?
-    // TODO: get rid of state variable
-    lrEncDistance  = (WHEEL_CIRC / WHEEL_STRIPES) * (double) leftCount;
-    rrEncDistance = (WHEEL_CIRC / WHEEL_STRIPES) * (double) rightCount;
+    // TODO 3 get rid of state variable
+    lrEncDistance  = ticksPerDist * (double) leftCount;
+    rrEncDistance = ticksPerDist * (double) rightCount;
     //encDistance = (lrEncDistance + rrEncDistance) / 2.0;
     // compute speed from time between ticks
     int leftTime = _left.readTime();
@@ -123,13 +153,13 @@
     if (leftTime <= 0) {
         lrEncSpeed = 0;
     } else {
-        lrEncSpeed = (2.0 * WHEEL_CIRC / WHEEL_STRIPES) / ((float) leftTime * 1e-6);
+        lrEncSpeed = (2.0 * ticksPerDist) / ((float) leftTime * 1e-6);
     }
     
     if (rightTime <= 0) {
         rrEncSpeed = 0;
     } else {
-        rrEncSpeed = (2.0 * WHEEL_CIRC / WHEEL_STRIPES) / ((float) rightTime * 1e-6);
+        rrEncSpeed = (2.0 * ticksPerDist) / ((float) rightTime * 1e-6);
     }
         
     // Dead band
@@ -168,8 +198,7 @@
 void Sensors::Read_Compass()
 {
     _compass.readMag(m);
-    // TODO: parameterize sign
-    // adjust for compass axis offsets and scale
+    // adjust for compass axis offsets, scale, and orientation (sign)
     for (int i=0; i < 3; i++) {
         mag[i] = ((float) m[i] - m_offset[i]) * m_scale[i] * m_sign[i];
     }
@@ -193,7 +222,7 @@
     for(int i=0; i < samples; i++) {  // We take some readings...
         Read_Gyro();
         Read_Accel();
-        wait(0.010); // sample at 100hz
+        wait(0.010); // sample at ~100hz
         for(int y=0; y < 3; y++) {   // accumulate values
             g_offset[y] += g[y];
             a_offset[y] += a[y];
@@ -205,7 +234,7 @@
         a_offset[y] /= samples;
     }
 
-    //TODO: if we ever get back to using accelerometer, do something about this.
+    //TODO 4 if we ever get back to using accelerometer, do something about this.
     //a_offset[_z_] -= GRAVITY * a_sign[_z_]; // I don't get why we're doing this...?
 }
 
@@ -254,7 +283,7 @@
     static float filter = -1.0;
     float v = _voltage * 3.3 * 4.12712;        // 242.3mV/V
 
-    // TODO: median filter to eliminate spikes
+    // TODO 3 median filter to eliminate spikes
     if (filter < 0) {
         filter = v * VFF;
     } else {
@@ -286,7 +315,7 @@
 
 void Sensors::Read_Power()
 {
-    // TODO: exponential or median filtering on these to get rid of spikes
+    // TODO 3 exponential or median filtering on these to get rid of spikes
     //
     voltage = getVoltage();
     current = getCurrent();