Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Sensors.cpp
00001 /* 00002 00003 MinIMU-9-Arduino-AHRS 00004 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) 00005 00006 Copyright (c) 2011 Pololu Corporation. 00007 http://www.pololu.com/ 00008 00009 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: 00010 http://code.google.com/p/sf9domahrs/ 00011 00012 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose 00013 Julio and Doug Weibel: 00014 http://code.google.com/p/ardu-imu/ 00015 00016 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it 00017 under the terms of the GNU Lesser General Public License as published by the 00018 Free Software Foundation, either version 3 of the License, or (at your option) 00019 any later version. 00020 00021 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but 00022 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 00023 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for 00024 more details. 00025 00026 You should have received a copy of the GNU Lesser General Public License along 00027 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. 00028 00029 */ 00030 00031 #include "Sensors.h" 00032 #include "debug.h" 00033 #include "stdio.h" 00034 00035 #define GYRO_SCALE 14.49787 // TODO: is the sign right here?? yes, see g_sign 00036 00037 #define VFF 50.0 // voltage filter factor 00038 00039 ///////////////////// MAGNETOMETER CALIBRATION 00040 00041 Sensors::Sensors(): 00042 _voltage(p19), // Voltage from sensor board 00043 _current(p20), // Current from sensor board 00044 _left(p30), // left wheel encoder 00045 _right(p29), // Right wheel encoder 00046 _gyro(p28, p27), // MinIMU-9 gyro 00047 _compass(p28, p27), // MinIMU-9 compass/accelerometer 00048 _rangers(p28, p27), // Arduino ADC to I2C 00049 _cam(p28, p27) 00050 { 00051 for (int i=0; i < 3; i++) { 00052 m_offset[i] = 0; 00053 m_scale[i] = 1; 00054 } 00055 00056 // TODO: parameterize 00057 g_scale[0] = 1; 00058 g_scale[1] = 1; 00059 g_scale[2] = GYRO_SCALE; 00060 00061 g_sign[0] = 1; 00062 g_sign[1] = -1; 00063 g_sign[2] = -1; 00064 00065 a_sign[0] = -1; 00066 a_sign[1] = 1; 00067 a_sign[2] = 1; 00068 00069 m_sign[0] = 1; 00070 m_sign[1] = -1; 00071 m_sign[2] = -1; 00072 00073 // upside down mounting 00074 //g_sign[3] = {1,1,1}; 00075 //a_sign[3] = {-1,-1,-1}; 00076 //m_sign[3] = {1,1,1}; 00077 } 00078 00079 /* Compass_Calibrate 00080 * 00081 * Set the offset and scale calibration for the compass 00082 */ 00083 void Sensors::Compass_Calibrate(float offset[3], float scale[3]) 00084 { 00085 for (int i=0; i < 3; i++) { 00086 m_offset[i] = offset[i]; 00087 m_scale[i] = scale[i]; 00088 } 00089 00090 return; 00091 } 00092 00093 00094 void Sensors::Read_Encoders() 00095 { 00096 // Odometry 00097 leftCount = _left.read(); 00098 rightCount = _right.read(); 00099 leftTotal += leftCount; 00100 rightTotal += rightCount; 00101 00102 // TODO--> sanity check on encoders; if difference between them 00103 // is huge, what do we do? Slipping wheel? Skidding wheel? 00104 // front encoders would be ideal as additional sanity check 00105 00106 // TODO: move this into scheduler?? 00107 00108 // TODO: how do we track distance, should we only check distance everytime we do a nav/pos update? 00109 // TODO: get rid of state variable 00110 lrEncDistance = (WHEEL_CIRC / WHEEL_STRIPES) * (double) leftCount; 00111 rrEncDistance = (WHEEL_CIRC / WHEEL_STRIPES) * (double) rightCount; 00112 encDistance = (lrEncDistance + rrEncDistance) / 2.0; 00113 // compute speed from time between ticks 00114 int leftTime = _left.readTime(); 00115 int rightTime = _right.readTime(); 00116 00117 // We could also supplement this with distance/time calcs once we get up to a higher 00118 // speed and encoder quantization error is lower 00119 // To reduce asymmetry of the encoder operation, we're only reading time 00120 // between rising ticks. So that means half the wheel stripes 00121 00122 if (leftTime <= 0) { 00123 lrEncSpeed = 0; 00124 } else { 00125 lrEncSpeed = (2.0 * WHEEL_CIRC / WHEEL_STRIPES) / ((float) leftTime * 1e-6); 00126 } 00127 00128 if (rightTime <= 0) { 00129 rrEncSpeed = 0; 00130 } else { 00131 rrEncSpeed = (2.0 * WHEEL_CIRC / WHEEL_STRIPES) / ((float) rightTime * 1e-6); 00132 } 00133 00134 // Dead band 00135 if (lrEncSpeed < 0.1) lrEncSpeed = 0; 00136 if (rrEncSpeed < 0.1) rrEncSpeed = 0; 00137 // TODO: 3 use more sophisticated approach where we count if any ticks have happened lately 00138 // and if not, reset the speed 00139 encSpeed = (lrEncSpeed + rrEncSpeed) / 2.0; 00140 } 00141 00142 00143 void Sensors::Read_Gyro() 00144 { 00145 _gyro.read(g); 00146 gTemp = (int) _gyro.readTemp(); 00147 00148 gyro[_x_] = g_sign[_x_] * (g[_x_] - g_offset[_x_]); // really need to scale this too 00149 gyro[_y_] = g_sign[_y_] * (g[_y_] - g_offset[_y_]); // really need to scale this too 00150 gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_]; 00151 00152 return; 00153 } 00154 00155 // Reads x,y and z accelerometer registers 00156 void Sensors::Read_Accel() 00157 { 00158 _compass.readAcc(a); 00159 00160 accel[_x_] = a_sign[_x_] * (a[_x_] - a_offset[_x_]); 00161 accel[_y_] = a_sign[_y_] * (a[_y_] - a_offset[_y_]); 00162 accel[_z_] = a_sign[_z_] * (a[_z_] - a_offset[_z_]); 00163 00164 return; 00165 } 00166 00167 void Sensors::Read_Compass() 00168 { 00169 _compass.readMag(m); 00170 // TODO: parameterize sign 00171 // adjust for compass axis offsets and scale 00172 for (int i=0; i < 3; i++) { 00173 mag[i] = ((float) m[i] - m_offset[i]) * m_scale[i] * m_sign[i]; 00174 } 00175 //fprintf(stdout, "m_offset=(%5.5f,%5.5f,%5.5f)\n", ahrs.c_magnetom[0],ahrs.c_magnetom[1],ahrs.c_magnetom[2]); 00176 00177 return; 00178 } 00179 00180 /* doing some crude gryo and accelerometer offset calibration here 00181 * 00182 */ 00183 void Sensors::Calculate_Offsets() 00184 { 00185 int samples=128; 00186 00187 for (int i=0; i < 3; i++) { 00188 g_offset[i] = 0; 00189 a_offset[i] = 0; 00190 } 00191 00192 for(int i=0; i < samples; i++) { // We take some readings... 00193 Read_Gyro(); 00194 Read_Accel(); 00195 for(int y=0; y < 3; y++) { // Cumulate values 00196 g_offset[y] += g[y]; 00197 a_offset[y] += a[y]; 00198 } 00199 wait(0.010); 00200 } 00201 00202 for(int y=0; y < 3; y++) { 00203 g_offset[y] /= samples; 00204 a_offset[y] /= samples; 00205 } 00206 00207 //TODO: if we ever get back to using accelerometer, do something about this. 00208 //a_offset[_z_] -= GRAVITY * a_sign[_z_]; // I don't get why we're doing this...? 00209 } 00210 00211 00212 00213 void Sensors::Compass_Heading() 00214 { 00215 /* 00216 float MAG_X; 00217 float MAG_Y; 00218 float cos_roll; 00219 float sin_roll; 00220 float cos_pitch; 00221 float sin_pitch; 00222 00223 // See DCM.cpp Drift_correction() 00224 00225 // umm... doesn't the dcm already have this info in it?! 00226 cos_roll = cos(ahrs.roll); 00227 sin_roll = sin(ahrs.roll); 00228 cos_pitch = cos(ahrs.pitch); 00229 sin_pitch = sin(ahrs.pitch); 00230 00231 // Tilt compensated Magnetic filed X: 00232 MAG_X = mag[_x_] * cos_pitch + mag[_y_] * sin_roll * sin_pitch + mag[_z_] * cos_roll * sin_pitch; 00233 // Tilt compensated Magnetic filed Y: 00234 MAG_Y = mag[_y_] * cos_roll - mag[_z_] * sin_roll; 00235 // Magnetic Heading 00236 ahrs.MAG_Heading = atan2(-MAG_Y,MAG_X); 00237 */ 00238 } 00239 00240 00241 void Sensors::getRawMag(int rawmag[3]) 00242 { 00243 Read_Compass(); 00244 for (int i=0; i < 3; i++) { 00245 rawmag[i] = m[i]; 00246 } 00247 } 00248 00249 00250 // getCurrent is a macro defined above 00251 00252 float Sensors::getVoltage() { 00253 static float filter = -1.0; 00254 float v = _voltage * 3.3 * 4.12712; // 242.3mV/V 00255 00256 // TODO: median filter to eliminate spikes 00257 if (filter < 0) { 00258 filter = v * VFF; 00259 } else { 00260 filter += (v - filter/VFF); 00261 } 00262 return (filter / VFF); 00263 } 00264 00265 00266 00267 float Sensors::getCurrent() { 00268 /*static bool first=true; 00269 static float history[3]; 00270 float tmp; 00271 float sort[3]; 00272 00273 if (first) { 00274 first = false; 00275 history[0] = history[1] = history[2] = _current; 00276 } else { 00277 sort[0] = history[0] = history[1]; 00278 sort[1] = history[1] = history[2]; 00279 sort[2] = history[2] = _current; 00280 } 00281 BubbleSort(sort, 3);*/ 00282 return (_current * 3.3 * 13.6612); // 73.20mV/A 00283 } 00284 00285 00286 void Sensors::Read_Power() 00287 { 00288 // TODO: exponential or median filtering on these to get rid of spikes 00289 // 00290 voltage = getVoltage(); 00291 current = getCurrent(); 00292 00293 return; 00294 } 00295 00296 float clampIRRanger(float x) 00297 { 00298 float y=x; 00299 00300 if (y < 0.1) 00301 y = 0.1; 00302 if (y > 5.0) 00303 y = 5.0; 00304 00305 return y; 00306 } 00307 00308 void Sensors::Read_Camera() 00309 { 00310 char count; 00311 char data[128]; 00312 char addr=0x80; 00313 /* 00314 struct { 00315 int color; 00316 int x1; 00317 int y1; 00318 int x2; 00319 int y2; 00320 } blob; 00321 */ 00322 00323 /* 00324 I2C START BIT 00325 WRITE: 0xA0 ACK 00326 WRITE: 0x00 ACK 00327 I2C START BIT 00328 WRITE: 0xA1 ACK 00329 READ: 0x01 ACK 0x02 ACK 0x03 ACK 0x04 ACK 0x05 ACK 0x06 ACK 0x07 ACK 0x08 ACK 0x09 ACK 0x0A 00330 NACK 00331 I2C STOP BIT 00332 */ 00333 00334 _cam.start(); 00335 _cam.write(0x11<<1); 00336 _cam.write(0x01); // command to send box data 00337 _cam.start(); 00338 _cam.write((0x11<<1 | 0x01)); 00339 count = _cam.read(1); // number of boxes tracked 00340 if (count > 8) count = 8; 00341 //fprintf(stdout, "----------\n%d\n", count); 00342 int x=0; 00343 for (int i=0; i < count; i++) { 00344 //fprintf(stdout, "%d: ", i); 00345 for (int j=0; j < 5; j++) { 00346 data[x] = _cam.read(1); 00347 //fprintf(stdout, "%d ", data[x]); 00348 x++; 00349 } 00350 //fprintf(stdout, "\n"); 00351 } 00352 _cam.read(0); // easiest to just read one more byte then NACK it 00353 _cam.stop(); 00354 } 00355 00356 00357 void Sensors::Read_Rangers() 00358 { 00359 char data[2]; 00360 // Sharp GP2Y0A710YK0F 00361 static float m=288.12; // slope adc=m(dist) + b 00362 static float b=219.3; 00363 static float m_per_lsb=0.004883/0.38583; // 0.004883mV/lsb * 0.0098in/lsb * 1in/0.0254m = m/lsb 00364 00365 _rangers.start(); 00366 data[0] = (0x11<<1 | 0x01); // send address + read = 1 00367 _rangers.write(data[0]); // send address 00368 ranger[0] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8; 00369 ranger[1] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8; 00370 ranger[2] = (int) _rangers.read(1) | ((int) _rangers.read(0)) << 8; // don't ack the last byte 00371 _rangers.stop(); 00372 00373 /* 00374 for (int q=0; q<3; q++) 00375 fprintf(stdout, "%04x ", ranger[q]); 00376 fprintf(stdout, "\n"); 00377 */ 00378 00379 if (ranger[0] < 86) { 00380 rightRanger = 999.0; 00381 } else if (ranger[0] > 585) { 00382 rightRanger = 20.0; 00383 } else { 00384 // Sharp GP2Y0A02YK0F 00385 rightRanger = 1/(8.664e-005*ranger[0]-0.0008); // IR distance, meters 00386 } 00387 00388 // Compute distances 00389 leftRanger = clampIRRanger( m/(ranger[1]-b) ); // IR distance, meters 00390 // Sonar 00391 centerRanger = ranger[2] * (m_per_lsb); // Sonar distance, metersmv/in=0.0098, mv/lsb=0.0049 00392 } 00393 00394 00395 00396 /** perform bubble sort 00397 * for median filtering 00398 */ 00399 void Sensors::BubbleSort(float *num, int numLength) 00400 { 00401 float temp; // holding variable 00402 bool flag=true; 00403 00404 for(int i = 1; (i <= numLength) && flag; i++) { 00405 flag = false; 00406 for (int j = 0; j < (numLength -1); j++) { 00407 if (num[j+1] > num[j]) { // ascending order simply changes to < 00408 temp = num[j]; // swap elements 00409 num[j] = num[j+1]; 00410 num[j+1] = temp; 00411 flag = true; 00412 } 00413 } 00414 } 00415 return; //arrays are passed to functions by address; nothing is returned 00416 }
Generated on Tue Jul 12 2022 14:09:28 by 1.7.2