Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Sensors/Sensors.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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