Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: Sensors/Sensors.cpp
- Revision:
- 12:5dfa1ab47838
- Parent:
- 2:fbc6e3cf3ed8
diff -r 33757b21d0e4 -r 5dfa1ab47838 Sensors/Sensors.cpp --- 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();