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@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
0:a6a169de725f
Child:
12:5dfa1ab47838
Sort-of working version, still some errors with estimation. Added clamp() function.

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