Michael Shimniok / Mbed 2 deprecated DataBus

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?

UserRevisionLine numberNew 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 }