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
Sensors/Sensors.cpp@25:bb5356402687, 2018-11-30 (annotated)
- Committer:
- shimniok
- Date:
- Fri Nov 30 16:11:53 2018 +0000
- Revision:
- 25:bb5356402687
- Parent:
- 12:5dfa1ab47838
Initial publish of revised version.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:a6a169de725f | 1 | /* |
shimniok | 0:a6a169de725f | 2 | |
shimniok | 0:a6a169de725f | 3 | MinIMU-9-Arduino-AHRS |
shimniok | 0:a6a169de725f | 4 | Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) |
shimniok | 0:a6a169de725f | 5 | |
shimniok | 0:a6a169de725f | 6 | Copyright (c) 2011 Pololu Corporation. |
shimniok | 0:a6a169de725f | 7 | http://www.pololu.com/ |
shimniok | 0:a6a169de725f | 8 | |
shimniok | 0:a6a169de725f | 9 | MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: |
shimniok | 0:a6a169de725f | 10 | http://code.google.com/p/sf9domahrs/ |
shimniok | 0:a6a169de725f | 11 | |
shimniok | 0:a6a169de725f | 12 | sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose |
shimniok | 0:a6a169de725f | 13 | Julio and Doug Weibel: |
shimniok | 0:a6a169de725f | 14 | http://code.google.com/p/ardu-imu/ |
shimniok | 0:a6a169de725f | 15 | |
shimniok | 0:a6a169de725f | 16 | MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it |
shimniok | 0:a6a169de725f | 17 | under the terms of the GNU Lesser General Public License as published by the |
shimniok | 0:a6a169de725f | 18 | Free Software Foundation, either version 3 of the License, or (at your option) |
shimniok | 0:a6a169de725f | 19 | any later version. |
shimniok | 0:a6a169de725f | 20 | |
shimniok | 0:a6a169de725f | 21 | MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but |
shimniok | 0:a6a169de725f | 22 | WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
shimniok | 0:a6a169de725f | 23 | FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for |
shimniok | 0:a6a169de725f | 24 | more details. |
shimniok | 0:a6a169de725f | 25 | |
shimniok | 0:a6a169de725f | 26 | You should have received a copy of the GNU Lesser General Public License along |
shimniok | 0:a6a169de725f | 27 | with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. |
shimniok | 0:a6a169de725f | 28 | |
shimniok | 0:a6a169de725f | 29 | */ |
shimniok | 0:a6a169de725f | 30 | |
shimniok | 12:5dfa1ab47838 | 31 | #include <stdio.h> |
shimniok | 12:5dfa1ab47838 | 32 | #include "globals.h" |
shimniok | 12:5dfa1ab47838 | 33 | #include "Config.h" |
shimniok | 12:5dfa1ab47838 | 34 | #include "devices.h" |
shimniok | 0:a6a169de725f | 35 | #include "Sensors.h" |
shimniok | 12:5dfa1ab47838 | 36 | #include "util.h" |
shimniok | 0:a6a169de725f | 37 | #include "debug.h" |
shimniok | 0:a6a169de725f | 38 | |
shimniok | 0:a6a169de725f | 39 | #define VFF 50.0 // voltage filter factor |
shimniok | 0:a6a169de725f | 40 | |
shimniok | 0:a6a169de725f | 41 | ///////////////////// MAGNETOMETER CALIBRATION |
shimniok | 0:a6a169de725f | 42 | |
shimniok | 0:a6a169de725f | 43 | Sensors::Sensors(): |
shimniok | 12:5dfa1ab47838 | 44 | gTemp(0), |
shimniok | 12:5dfa1ab47838 | 45 | leftRanger(0), |
shimniok | 12:5dfa1ab47838 | 46 | rightRanger(0), |
shimniok | 12:5dfa1ab47838 | 47 | centerRanger(0), |
shimniok | 12:5dfa1ab47838 | 48 | voltage(0), |
shimniok | 12:5dfa1ab47838 | 49 | current(0), |
shimniok | 12:5dfa1ab47838 | 50 | leftTotal(0), |
shimniok | 12:5dfa1ab47838 | 51 | rightTotal(0), |
shimniok | 12:5dfa1ab47838 | 52 | leftCount(0), |
shimniok | 12:5dfa1ab47838 | 53 | rightCount(0), |
shimniok | 12:5dfa1ab47838 | 54 | lrEncDistance(0.0), |
shimniok | 12:5dfa1ab47838 | 55 | rrEncDistance(0.0), |
shimniok | 12:5dfa1ab47838 | 56 | lrEncSpeed(0.0), |
shimniok | 12:5dfa1ab47838 | 57 | rrEncSpeed(0.0), |
shimniok | 12:5dfa1ab47838 | 58 | encDistance(0.0), |
shimniok | 12:5dfa1ab47838 | 59 | encSpeed(0.0), |
shimniok | 12:5dfa1ab47838 | 60 | gps(GPSTX, GPSRX), |
shimniok | 0:a6a169de725f | 61 | _voltage(p19), // Voltage from sensor board |
shimniok | 0:a6a169de725f | 62 | _current(p20), // Current from sensor board |
shimniok | 12:5dfa1ab47838 | 63 | _left(ENCALEFT), // left wheel encoder |
shimniok | 12:5dfa1ab47838 | 64 | _right(ENCARIGHT), // Right wheel encoder |
shimniok | 12:5dfa1ab47838 | 65 | _gyro(I2CSDA, I2CSCL), // MinIMU-9 gyro |
shimniok | 12:5dfa1ab47838 | 66 | _compass(I2CSDA, I2CSCL), // MinIMU-9 compass/accelerometer |
shimniok | 12:5dfa1ab47838 | 67 | _rangers(I2CSDA, I2CSCL), // Arduino ADC to I2C |
shimniok | 12:5dfa1ab47838 | 68 | _cam(I2CSDA, I2CSCL) |
shimniok | 12:5dfa1ab47838 | 69 | , _tireCirc(0) |
shimniok | 12:5dfa1ab47838 | 70 | , _encStripes(0) |
shimniok | 0:a6a169de725f | 71 | { |
shimniok | 0:a6a169de725f | 72 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 73 | m_offset[i] = 0; |
shimniok | 0:a6a169de725f | 74 | m_scale[i] = 1; |
shimniok | 0:a6a169de725f | 75 | } |
shimniok | 0:a6a169de725f | 76 | |
shimniok | 12:5dfa1ab47838 | 77 | // TODO 2 parameterize scale and sign for mag, accel, gyro |
shimniok | 12:5dfa1ab47838 | 78 | g_scale[_x_] = 1; |
shimniok | 12:5dfa1ab47838 | 79 | g_scale[_y_] = 1; |
shimniok | 12:5dfa1ab47838 | 80 | g_scale[_z_] = 1; |
shimniok | 0:a6a169de725f | 81 | |
shimniok | 12:5dfa1ab47838 | 82 | g_sign[_x_] = 1; |
shimniok | 12:5dfa1ab47838 | 83 | g_sign[_y_] = -1; |
shimniok | 12:5dfa1ab47838 | 84 | g_sign[_z_] = -1; |
shimniok | 0:a6a169de725f | 85 | |
shimniok | 12:5dfa1ab47838 | 86 | a_sign[_x_] = -1; |
shimniok | 12:5dfa1ab47838 | 87 | a_sign[_y_] = 1; |
shimniok | 12:5dfa1ab47838 | 88 | a_sign[_z_] = 1; |
shimniok | 0:a6a169de725f | 89 | |
shimniok | 12:5dfa1ab47838 | 90 | m_sign[_x_] = 1; |
shimniok | 12:5dfa1ab47838 | 91 | m_sign[_y_] = -1; |
shimniok | 12:5dfa1ab47838 | 92 | m_sign[_z_] = -1; |
shimniok | 0:a6a169de725f | 93 | |
shimniok | 0:a6a169de725f | 94 | // upside down mounting |
shimniok | 0:a6a169de725f | 95 | //g_sign[3] = {1,1,1}; |
shimniok | 0:a6a169de725f | 96 | //a_sign[3] = {-1,-1,-1}; |
shimniok | 0:a6a169de725f | 97 | //m_sign[3] = {1,1,1}; |
shimniok | 0:a6a169de725f | 98 | } |
shimniok | 0:a6a169de725f | 99 | |
shimniok | 12:5dfa1ab47838 | 100 | void Sensors::setGyroScale(float scale) { |
shimniok | 12:5dfa1ab47838 | 101 | g_scale[_z_] = scale; |
shimniok | 12:5dfa1ab47838 | 102 | } |
shimniok | 12:5dfa1ab47838 | 103 | |
shimniok | 12:5dfa1ab47838 | 104 | |
shimniok | 0:a6a169de725f | 105 | /* Compass_Calibrate |
shimniok | 0:a6a169de725f | 106 | * |
shimniok | 0:a6a169de725f | 107 | * Set the offset and scale calibration for the compass |
shimniok | 0:a6a169de725f | 108 | */ |
shimniok | 0:a6a169de725f | 109 | void Sensors::Compass_Calibrate(float offset[3], float scale[3]) |
shimniok | 0:a6a169de725f | 110 | { |
shimniok | 0:a6a169de725f | 111 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 112 | m_offset[i] = offset[i]; |
shimniok | 0:a6a169de725f | 113 | m_scale[i] = scale[i]; |
shimniok | 0:a6a169de725f | 114 | } |
shimniok | 0:a6a169de725f | 115 | |
shimniok | 0:a6a169de725f | 116 | return; |
shimniok | 0:a6a169de725f | 117 | } |
shimniok | 0:a6a169de725f | 118 | |
shimniok | 12:5dfa1ab47838 | 119 | void Sensors::configureEncoders(float tireCirc, int encStripes) |
shimniok | 12:5dfa1ab47838 | 120 | { |
shimniok | 12:5dfa1ab47838 | 121 | _tireCirc = tireCirc; |
shimniok | 12:5dfa1ab47838 | 122 | _encStripes = encStripes; |
shimniok | 12:5dfa1ab47838 | 123 | } |
shimniok | 12:5dfa1ab47838 | 124 | |
shimniok | 0:a6a169de725f | 125 | |
shimniok | 0:a6a169de725f | 126 | void Sensors::Read_Encoders() |
shimniok | 0:a6a169de725f | 127 | { |
shimniok | 0:a6a169de725f | 128 | // Odometry |
shimniok | 0:a6a169de725f | 129 | leftCount = _left.read(); |
shimniok | 0:a6a169de725f | 130 | rightCount = _right.read(); |
shimniok | 0:a6a169de725f | 131 | leftTotal += leftCount; |
shimniok | 0:a6a169de725f | 132 | rightTotal += rightCount; |
shimniok | 12:5dfa1ab47838 | 133 | |
shimniok | 12:5dfa1ab47838 | 134 | float ticksPerDist = _tireCirc / _encStripes; |
shimniok | 12:5dfa1ab47838 | 135 | |
shimniok | 12:5dfa1ab47838 | 136 | // TODO 3 sanity check on encoders; if difference between them |
shimniok | 12:5dfa1ab47838 | 137 | // is huge, what do we do? Slipping wheel? Skidding wheel? Broken encoder? |
shimniok | 0:a6a169de725f | 138 | // front encoders would be ideal as additional sanity check |
shimniok | 0:a6a169de725f | 139 | |
shimniok | 12:5dfa1ab47838 | 140 | // TODO 3 get rid of state variable |
shimniok | 12:5dfa1ab47838 | 141 | lrEncDistance = ticksPerDist * (double) leftCount; |
shimniok | 12:5dfa1ab47838 | 142 | rrEncDistance = ticksPerDist * (double) rightCount; |
shimniok | 2:fbc6e3cf3ed8 | 143 | //encDistance = (lrEncDistance + rrEncDistance) / 2.0; |
shimniok | 0:a6a169de725f | 144 | // compute speed from time between ticks |
shimniok | 0:a6a169de725f | 145 | int leftTime = _left.readTime(); |
shimniok | 0:a6a169de725f | 146 | int rightTime = _right.readTime(); |
shimniok | 0:a6a169de725f | 147 | |
shimniok | 0:a6a169de725f | 148 | // We could also supplement this with distance/time calcs once we get up to a higher |
shimniok | 0:a6a169de725f | 149 | // speed and encoder quantization error is lower |
shimniok | 0:a6a169de725f | 150 | // To reduce asymmetry of the encoder operation, we're only reading time |
shimniok | 0:a6a169de725f | 151 | // between rising ticks. So that means half the wheel stripes |
shimniok | 0:a6a169de725f | 152 | |
shimniok | 0:a6a169de725f | 153 | if (leftTime <= 0) { |
shimniok | 0:a6a169de725f | 154 | lrEncSpeed = 0; |
shimniok | 0:a6a169de725f | 155 | } else { |
shimniok | 12:5dfa1ab47838 | 156 | lrEncSpeed = (2.0 * ticksPerDist) / ((float) leftTime * 1e-6); |
shimniok | 0:a6a169de725f | 157 | } |
shimniok | 0:a6a169de725f | 158 | |
shimniok | 0:a6a169de725f | 159 | if (rightTime <= 0) { |
shimniok | 0:a6a169de725f | 160 | rrEncSpeed = 0; |
shimniok | 0:a6a169de725f | 161 | } else { |
shimniok | 12:5dfa1ab47838 | 162 | rrEncSpeed = (2.0 * ticksPerDist) / ((float) rightTime * 1e-6); |
shimniok | 0:a6a169de725f | 163 | } |
shimniok | 0:a6a169de725f | 164 | |
shimniok | 0:a6a169de725f | 165 | // Dead band |
shimniok | 0:a6a169de725f | 166 | if (lrEncSpeed < 0.1) lrEncSpeed = 0; |
shimniok | 0:a6a169de725f | 167 | if (rrEncSpeed < 0.1) rrEncSpeed = 0; |
shimniok | 0:a6a169de725f | 168 | // TODO: 3 use more sophisticated approach where we count if any ticks have happened lately |
shimniok | 0:a6a169de725f | 169 | // and if not, reset the speed |
shimniok | 0:a6a169de725f | 170 | encSpeed = (lrEncSpeed + rrEncSpeed) / 2.0; |
shimniok | 0:a6a169de725f | 171 | } |
shimniok | 0:a6a169de725f | 172 | |
shimniok | 0:a6a169de725f | 173 | |
shimniok | 0:a6a169de725f | 174 | void Sensors::Read_Gyro() |
shimniok | 0:a6a169de725f | 175 | { |
shimniok | 0:a6a169de725f | 176 | _gyro.read(g); |
shimniok | 0:a6a169de725f | 177 | gTemp = (int) _gyro.readTemp(); |
shimniok | 0:a6a169de725f | 178 | |
shimniok | 0:a6a169de725f | 179 | gyro[_x_] = g_sign[_x_] * (g[_x_] - g_offset[_x_]); // really need to scale this too |
shimniok | 0:a6a169de725f | 180 | gyro[_y_] = g_sign[_y_] * (g[_y_] - g_offset[_y_]); // really need to scale this too |
shimniok | 0:a6a169de725f | 181 | gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_]; |
shimniok | 0:a6a169de725f | 182 | |
shimniok | 0:a6a169de725f | 183 | return; |
shimniok | 0:a6a169de725f | 184 | } |
shimniok | 0:a6a169de725f | 185 | |
shimniok | 0:a6a169de725f | 186 | // Reads x,y and z accelerometer registers |
shimniok | 0:a6a169de725f | 187 | void Sensors::Read_Accel() |
shimniok | 0:a6a169de725f | 188 | { |
shimniok | 0:a6a169de725f | 189 | _compass.readAcc(a); |
shimniok | 0:a6a169de725f | 190 | |
shimniok | 0:a6a169de725f | 191 | accel[_x_] = a_sign[_x_] * (a[_x_] - a_offset[_x_]); |
shimniok | 0:a6a169de725f | 192 | accel[_y_] = a_sign[_y_] * (a[_y_] - a_offset[_y_]); |
shimniok | 0:a6a169de725f | 193 | accel[_z_] = a_sign[_z_] * (a[_z_] - a_offset[_z_]); |
shimniok | 0:a6a169de725f | 194 | |
shimniok | 0:a6a169de725f | 195 | return; |
shimniok | 0:a6a169de725f | 196 | } |
shimniok | 0:a6a169de725f | 197 | |
shimniok | 0:a6a169de725f | 198 | void Sensors::Read_Compass() |
shimniok | 0:a6a169de725f | 199 | { |
shimniok | 0:a6a169de725f | 200 | _compass.readMag(m); |
shimniok | 12:5dfa1ab47838 | 201 | // adjust for compass axis offsets, scale, and orientation (sign) |
shimniok | 0:a6a169de725f | 202 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 203 | mag[i] = ((float) m[i] - m_offset[i]) * m_scale[i] * m_sign[i]; |
shimniok | 0:a6a169de725f | 204 | } |
shimniok | 0:a6a169de725f | 205 | //fprintf(stdout, "m_offset=(%5.5f,%5.5f,%5.5f)\n", ahrs.c_magnetom[0],ahrs.c_magnetom[1],ahrs.c_magnetom[2]); |
shimniok | 0:a6a169de725f | 206 | |
shimniok | 0:a6a169de725f | 207 | return; |
shimniok | 0:a6a169de725f | 208 | } |
shimniok | 0:a6a169de725f | 209 | |
shimniok | 0:a6a169de725f | 210 | /* doing some crude gryo and accelerometer offset calibration here |
shimniok | 0:a6a169de725f | 211 | * |
shimniok | 0:a6a169de725f | 212 | */ |
shimniok | 0:a6a169de725f | 213 | void Sensors::Calculate_Offsets() |
shimniok | 0:a6a169de725f | 214 | { |
shimniok | 0:a6a169de725f | 215 | int samples=128; |
shimniok | 0:a6a169de725f | 216 | |
shimniok | 0:a6a169de725f | 217 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 218 | g_offset[i] = 0; |
shimniok | 0:a6a169de725f | 219 | a_offset[i] = 0; |
shimniok | 0:a6a169de725f | 220 | } |
shimniok | 0:a6a169de725f | 221 | |
shimniok | 0:a6a169de725f | 222 | for(int i=0; i < samples; i++) { // We take some readings... |
shimniok | 0:a6a169de725f | 223 | Read_Gyro(); |
shimniok | 0:a6a169de725f | 224 | Read_Accel(); |
shimniok | 12:5dfa1ab47838 | 225 | wait(0.010); // sample at ~100hz |
shimniok | 0:a6a169de725f | 226 | for(int y=0; y < 3; y++) { // accumulate values |
shimniok | 0:a6a169de725f | 227 | g_offset[y] += g[y]; |
shimniok | 0:a6a169de725f | 228 | a_offset[y] += a[y]; |
shimniok | 0:a6a169de725f | 229 | } |
shimniok | 0:a6a169de725f | 230 | } |
shimniok | 0:a6a169de725f | 231 | |
shimniok | 0:a6a169de725f | 232 | for(int y=0; y < 3; y++) { |
shimniok | 0:a6a169de725f | 233 | g_offset[y] /= samples; |
shimniok | 0:a6a169de725f | 234 | a_offset[y] /= samples; |
shimniok | 0:a6a169de725f | 235 | } |
shimniok | 0:a6a169de725f | 236 | |
shimniok | 12:5dfa1ab47838 | 237 | //TODO 4 if we ever get back to using accelerometer, do something about this. |
shimniok | 0:a6a169de725f | 238 | //a_offset[_z_] -= GRAVITY * a_sign[_z_]; // I don't get why we're doing this...? |
shimniok | 0:a6a169de725f | 239 | } |
shimniok | 0:a6a169de725f | 240 | |
shimniok | 0:a6a169de725f | 241 | |
shimniok | 0:a6a169de725f | 242 | |
shimniok | 0:a6a169de725f | 243 | void Sensors::Compass_Heading() |
shimniok | 0:a6a169de725f | 244 | { |
shimniok | 0:a6a169de725f | 245 | /* |
shimniok | 0:a6a169de725f | 246 | float MAG_X; |
shimniok | 0:a6a169de725f | 247 | float MAG_Y; |
shimniok | 0:a6a169de725f | 248 | float cos_roll; |
shimniok | 0:a6a169de725f | 249 | float sin_roll; |
shimniok | 0:a6a169de725f | 250 | float cos_pitch; |
shimniok | 0:a6a169de725f | 251 | float sin_pitch; |
shimniok | 0:a6a169de725f | 252 | |
shimniok | 0:a6a169de725f | 253 | // See DCM.cpp Drift_correction() |
shimniok | 0:a6a169de725f | 254 | |
shimniok | 0:a6a169de725f | 255 | // umm... doesn't the dcm already have this info in it?! |
shimniok | 0:a6a169de725f | 256 | cos_roll = cos(ahrs.roll); |
shimniok | 0:a6a169de725f | 257 | sin_roll = sin(ahrs.roll); |
shimniok | 0:a6a169de725f | 258 | cos_pitch = cos(ahrs.pitch); |
shimniok | 0:a6a169de725f | 259 | sin_pitch = sin(ahrs.pitch); |
shimniok | 0:a6a169de725f | 260 | |
shimniok | 0:a6a169de725f | 261 | // Tilt compensated Magnetic filed X: |
shimniok | 0:a6a169de725f | 262 | MAG_X = mag[_x_] * cos_pitch + mag[_y_] * sin_roll * sin_pitch + mag[_z_] * cos_roll * sin_pitch; |
shimniok | 0:a6a169de725f | 263 | // Tilt compensated Magnetic filed Y: |
shimniok | 0:a6a169de725f | 264 | MAG_Y = mag[_y_] * cos_roll - mag[_z_] * sin_roll; |
shimniok | 0:a6a169de725f | 265 | // Magnetic Heading |
shimniok | 0:a6a169de725f | 266 | ahrs.MAG_Heading = atan2(-MAG_Y,MAG_X); |
shimniok | 0:a6a169de725f | 267 | */ |
shimniok | 0:a6a169de725f | 268 | } |
shimniok | 0:a6a169de725f | 269 | |
shimniok | 0:a6a169de725f | 270 | |
shimniok | 0:a6a169de725f | 271 | void Sensors::getRawMag(int rawmag[3]) |
shimniok | 0:a6a169de725f | 272 | { |
shimniok | 0:a6a169de725f | 273 | Read_Compass(); |
shimniok | 0:a6a169de725f | 274 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 275 | rawmag[i] = m[i]; |
shimniok | 0:a6a169de725f | 276 | } |
shimniok | 0:a6a169de725f | 277 | } |
shimniok | 0:a6a169de725f | 278 | |
shimniok | 0:a6a169de725f | 279 | |
shimniok | 0:a6a169de725f | 280 | // getCurrent is a macro defined above |
shimniok | 0:a6a169de725f | 281 | |
shimniok | 0:a6a169de725f | 282 | float Sensors::getVoltage() { |
shimniok | 0:a6a169de725f | 283 | static float filter = -1.0; |
shimniok | 0:a6a169de725f | 284 | float v = _voltage * 3.3 * 4.12712; // 242.3mV/V |
shimniok | 0:a6a169de725f | 285 | |
shimniok | 12:5dfa1ab47838 | 286 | // TODO 3 median filter to eliminate spikes |
shimniok | 0:a6a169de725f | 287 | if (filter < 0) { |
shimniok | 0:a6a169de725f | 288 | filter = v * VFF; |
shimniok | 0:a6a169de725f | 289 | } else { |
shimniok | 0:a6a169de725f | 290 | filter += (v - filter/VFF); |
shimniok | 0:a6a169de725f | 291 | } |
shimniok | 0:a6a169de725f | 292 | return (filter / VFF); |
shimniok | 0:a6a169de725f | 293 | } |
shimniok | 0:a6a169de725f | 294 | |
shimniok | 0:a6a169de725f | 295 | |
shimniok | 0:a6a169de725f | 296 | |
shimniok | 0:a6a169de725f | 297 | float Sensors::getCurrent() { |
shimniok | 0:a6a169de725f | 298 | /*static bool first=true; |
shimniok | 0:a6a169de725f | 299 | static float history[3]; |
shimniok | 0:a6a169de725f | 300 | float tmp; |
shimniok | 0:a6a169de725f | 301 | float sort[3]; |
shimniok | 0:a6a169de725f | 302 | |
shimniok | 0:a6a169de725f | 303 | if (first) { |
shimniok | 0:a6a169de725f | 304 | first = false; |
shimniok | 0:a6a169de725f | 305 | history[0] = history[1] = history[2] = _current; |
shimniok | 0:a6a169de725f | 306 | } else { |
shimniok | 0:a6a169de725f | 307 | sort[0] = history[0] = history[1]; |
shimniok | 0:a6a169de725f | 308 | sort[1] = history[1] = history[2]; |
shimniok | 0:a6a169de725f | 309 | sort[2] = history[2] = _current; |
shimniok | 0:a6a169de725f | 310 | } |
shimniok | 0:a6a169de725f | 311 | BubbleSort(sort, 3);*/ |
shimniok | 0:a6a169de725f | 312 | return (_current * 3.3 * 13.6612); // 73.20mV/A |
shimniok | 0:a6a169de725f | 313 | } |
shimniok | 0:a6a169de725f | 314 | |
shimniok | 0:a6a169de725f | 315 | |
shimniok | 0:a6a169de725f | 316 | void Sensors::Read_Power() |
shimniok | 0:a6a169de725f | 317 | { |
shimniok | 12:5dfa1ab47838 | 318 | // TODO 3 exponential or median filtering on these to get rid of spikes |
shimniok | 0:a6a169de725f | 319 | // |
shimniok | 0:a6a169de725f | 320 | voltage = getVoltage(); |
shimniok | 0:a6a169de725f | 321 | current = getCurrent(); |
shimniok | 0:a6a169de725f | 322 | |
shimniok | 0:a6a169de725f | 323 | return; |
shimniok | 0:a6a169de725f | 324 | } |
shimniok | 0:a6a169de725f | 325 | |
shimniok | 0:a6a169de725f | 326 | float clampIRRanger(float x) |
shimniok | 0:a6a169de725f | 327 | { |
shimniok | 0:a6a169de725f | 328 | float y=x; |
shimniok | 0:a6a169de725f | 329 | |
shimniok | 0:a6a169de725f | 330 | if (y < 0.1) |
shimniok | 0:a6a169de725f | 331 | y = 0.1; |
shimniok | 0:a6a169de725f | 332 | if (y > 5.0) |
shimniok | 0:a6a169de725f | 333 | y = 5.0; |
shimniok | 0:a6a169de725f | 334 | |
shimniok | 0:a6a169de725f | 335 | return y; |
shimniok | 0:a6a169de725f | 336 | } |
shimniok | 0:a6a169de725f | 337 | |
shimniok | 0:a6a169de725f | 338 | void Sensors::Read_Camera() |
shimniok | 0:a6a169de725f | 339 | { |
shimniok | 0:a6a169de725f | 340 | char count; |
shimniok | 0:a6a169de725f | 341 | //char data[128]; |
shimniok | 0:a6a169de725f | 342 | //char addr=0x80; |
shimniok | 0:a6a169de725f | 343 | /* |
shimniok | 0:a6a169de725f | 344 | struct { |
shimniok | 0:a6a169de725f | 345 | int color; |
shimniok | 0:a6a169de725f | 346 | int x1; |
shimniok | 0:a6a169de725f | 347 | int y1; |
shimniok | 0:a6a169de725f | 348 | int x2; |
shimniok | 0:a6a169de725f | 349 | int y2; |
shimniok | 0:a6a169de725f | 350 | } blob; |
shimniok | 0:a6a169de725f | 351 | */ |
shimniok | 0:a6a169de725f | 352 | |
shimniok | 0:a6a169de725f | 353 | /* |
shimniok | 0:a6a169de725f | 354 | I2C START BIT |
shimniok | 0:a6a169de725f | 355 | WRITE: 0xA0 ACK |
shimniok | 0:a6a169de725f | 356 | WRITE: 0x00 ACK |
shimniok | 0:a6a169de725f | 357 | I2C START BIT |
shimniok | 0:a6a169de725f | 358 | WRITE: 0xA1 ACK |
shimniok | 0:a6a169de725f | 359 | READ: 0x01 ACK 0x02 ACK 0x03 ACK 0x04 ACK 0x05 ACK 0x06 ACK 0x07 ACK 0x08 ACK 0x09 ACK 0x0A |
shimniok | 0:a6a169de725f | 360 | NACK |
shimniok | 0:a6a169de725f | 361 | I2C STOP BIT |
shimniok | 0:a6a169de725f | 362 | */ |
shimniok | 0:a6a169de725f | 363 | |
shimniok | 0:a6a169de725f | 364 | _cam.start(); |
shimniok | 0:a6a169de725f | 365 | _cam.write(0x11<<1); |
shimniok | 0:a6a169de725f | 366 | _cam.write(0x01); // command to send box data |
shimniok | 0:a6a169de725f | 367 | _cam.start(); |
shimniok | 0:a6a169de725f | 368 | _cam.write((0x11<<1 | 0x01)); |
shimniok | 0:a6a169de725f | 369 | count = _cam.read(1); // number of boxes tracked |
shimniok | 0:a6a169de725f | 370 | if (count > 8) count = 8; |
shimniok | 0:a6a169de725f | 371 | //fprintf(stdout, "----------\n%d\n", count); |
shimniok | 0:a6a169de725f | 372 | int x=0; |
shimniok | 0:a6a169de725f | 373 | for (int i=0; i < count; i++) { |
shimniok | 0:a6a169de725f | 374 | //fprintf(stdout, "%d: ", i); |
shimniok | 0:a6a169de725f | 375 | for (int j=0; j < 5; j++) { |
shimniok | 0:a6a169de725f | 376 | //data[x] = _cam.read(1); |
shimniok | 0:a6a169de725f | 377 | //fprintf(stdout, "%d ", data[x]); |
shimniok | 0:a6a169de725f | 378 | x++; |
shimniok | 0:a6a169de725f | 379 | } |
shimniok | 0:a6a169de725f | 380 | //fprintf(stdout, "\n"); |
shimniok | 0:a6a169de725f | 381 | } |
shimniok | 0:a6a169de725f | 382 | _cam.read(0); // easiest to just read one more byte then NACK it |
shimniok | 0:a6a169de725f | 383 | _cam.stop(); |
shimniok | 0:a6a169de725f | 384 | } |
shimniok | 0:a6a169de725f | 385 | |
shimniok | 0:a6a169de725f | 386 | |
shimniok | 0:a6a169de725f | 387 | void Sensors::Read_Rangers() |
shimniok | 0:a6a169de725f | 388 | { |
shimniok | 0:a6a169de725f | 389 | char data[2]; |
shimniok | 0:a6a169de725f | 390 | // Sharp GP2Y0A710YK0F |
shimniok | 0:a6a169de725f | 391 | static float m=288.12; // slope adc=m(dist) + b |
shimniok | 0:a6a169de725f | 392 | static float b=219.3; |
shimniok | 0:a6a169de725f | 393 | static float m_per_lsb=0.004883/0.38583; // 0.004883mV/lsb * 0.0098in/lsb * 1in/0.0254m = m/lsb |
shimniok | 0:a6a169de725f | 394 | |
shimniok | 0:a6a169de725f | 395 | _rangers.start(); |
shimniok | 0:a6a169de725f | 396 | data[0] = (0x11<<1 | 0x01); // send address + read = 1 |
shimniok | 0:a6a169de725f | 397 | _rangers.write(data[0]); // send address |
shimniok | 0:a6a169de725f | 398 | ranger[0] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8; |
shimniok | 0:a6a169de725f | 399 | ranger[1] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8; |
shimniok | 0:a6a169de725f | 400 | ranger[2] = (int) _rangers.read(1) | ((int) _rangers.read(0)) << 8; // don't ack the last byte |
shimniok | 0:a6a169de725f | 401 | _rangers.stop(); |
shimniok | 0:a6a169de725f | 402 | |
shimniok | 0:a6a169de725f | 403 | /* |
shimniok | 0:a6a169de725f | 404 | for (int q=0; q<3; q++) |
shimniok | 0:a6a169de725f | 405 | fprintf(stdout, "%04x ", ranger[q]); |
shimniok | 0:a6a169de725f | 406 | fprintf(stdout, "\n"); |
shimniok | 0:a6a169de725f | 407 | */ |
shimniok | 0:a6a169de725f | 408 | |
shimniok | 0:a6a169de725f | 409 | if (ranger[0] < 86) { |
shimniok | 0:a6a169de725f | 410 | rightRanger = 999.0; |
shimniok | 0:a6a169de725f | 411 | } else if (ranger[0] > 585) { |
shimniok | 0:a6a169de725f | 412 | rightRanger = 20.0; |
shimniok | 0:a6a169de725f | 413 | } else { |
shimniok | 0:a6a169de725f | 414 | // Sharp GP2Y0A02YK0F |
shimniok | 0:a6a169de725f | 415 | rightRanger = 1/(8.664e-005*ranger[0]-0.0008); // IR distance, meters |
shimniok | 0:a6a169de725f | 416 | } |
shimniok | 0:a6a169de725f | 417 | |
shimniok | 0:a6a169de725f | 418 | // Compute distances |
shimniok | 0:a6a169de725f | 419 | leftRanger = clampIRRanger( m/(ranger[1]-b) ); // IR distance, meters |
shimniok | 0:a6a169de725f | 420 | // Sonar |
shimniok | 0:a6a169de725f | 421 | centerRanger = ranger[2] * (m_per_lsb); // Sonar distance, metersmv/in=0.0098, mv/lsb=0.0049 |
shimniok | 0:a6a169de725f | 422 | } |
shimniok | 0:a6a169de725f | 423 | |
shimniok | 0:a6a169de725f | 424 | |
shimniok | 0:a6a169de725f | 425 | |
shimniok | 0:a6a169de725f | 426 | /** perform bubble sort |
shimniok | 0:a6a169de725f | 427 | * for median filtering |
shimniok | 0:a6a169de725f | 428 | */ |
shimniok | 0:a6a169de725f | 429 | void Sensors::BubbleSort(float *num, int numLength) |
shimniok | 0:a6a169de725f | 430 | { |
shimniok | 0:a6a169de725f | 431 | float temp; // holding variable |
shimniok | 0:a6a169de725f | 432 | bool flag=true; |
shimniok | 0:a6a169de725f | 433 | |
shimniok | 0:a6a169de725f | 434 | for(int i = 1; (i <= numLength) && flag; i++) { |
shimniok | 0:a6a169de725f | 435 | flag = false; |
shimniok | 0:a6a169de725f | 436 | for (int j = 0; j < (numLength -1); j++) { |
shimniok | 0:a6a169de725f | 437 | if (num[j+1] > num[j]) { // ascending order simply changes to < |
shimniok | 0:a6a169de725f | 438 | temp = num[j]; // swap elements |
shimniok | 0:a6a169de725f | 439 | num[j] = num[j+1]; |
shimniok | 0:a6a169de725f | 440 | num[j+1] = temp; |
shimniok | 0:a6a169de725f | 441 | flag = true; |
shimniok | 0:a6a169de725f | 442 | } |
shimniok | 0:a6a169de725f | 443 | } |
shimniok | 0:a6a169de725f | 444 | } |
shimniok | 0:a6a169de725f | 445 | return; //arrays are passed to functions by address; nothing is returned |
shimniok | 0:a6a169de725f | 446 | } |