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@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
2:fbc6e3cf3ed8
Working version with priorities set and update time display

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