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 <stdio.h>
00032 #include "globals.h"
00033 #include "Config.h"
00034 #include "devices.h"
00035 #include "Sensors.h"
00036 #include "util.h"
00037 #include "debug.h"
00038 
00039 #define VFF 50.0 // voltage filter factor
00040 
00041 ///////////////////// MAGNETOMETER CALIBRATION
00042 
00043 Sensors::Sensors():
00044     gTemp(0),
00045     leftRanger(0),
00046     rightRanger(0),
00047     centerRanger(0),
00048     voltage(0),
00049     current(0),
00050     leftTotal(0),
00051     rightTotal(0),
00052     leftCount(0),
00053     rightCount(0),
00054     lrEncDistance(0.0),
00055     rrEncDistance(0.0),
00056     lrEncSpeed(0.0),
00057     rrEncSpeed(0.0),
00058     encDistance(0.0),
00059     encSpeed(0.0),
00060     gps(GPSTX, GPSRX),
00061     _voltage(p19),               // Voltage from sensor board
00062     _current(p20),               // Current from sensor board
00063     _left(ENCALEFT),                  // left wheel encoder
00064     _right(ENCARIGHT),                 // Right wheel encoder
00065     _gyro(I2CSDA, I2CSCL),       // MinIMU-9 gyro
00066     _compass(I2CSDA, I2CSCL),    // MinIMU-9 compass/accelerometer
00067     _rangers(I2CSDA, I2CSCL),    // Arduino ADC to I2C
00068     _cam(I2CSDA, I2CSCL)
00069 ,   _tireCirc(0)
00070 ,   _encStripes(0)
00071 {
00072     for (int i=0; i < 3; i++) {
00073         m_offset[i] = 0;
00074         m_scale[i] = 1;
00075     }
00076 
00077     // TODO 2 parameterize scale and sign for mag, accel, gyro
00078     g_scale[_x_] = 1;
00079     g_scale[_y_] = 1;
00080     g_scale[_z_] = 1;
00081 
00082     g_sign[_x_] = 1;
00083     g_sign[_y_] = -1;
00084     g_sign[_z_] = -1;
00085 
00086     a_sign[_x_] = -1;
00087     a_sign[_y_] = 1;
00088     a_sign[_z_] = 1;
00089 
00090     m_sign[_x_] = 1;
00091     m_sign[_y_] = -1;
00092     m_sign[_z_] = -1;
00093 
00094     // upside down mounting
00095     //g_sign[3] = {1,1,1};        
00096     //a_sign[3] = {-1,-1,-1};
00097     //m_sign[3] = {1,1,1}; 
00098 }
00099 
00100 void Sensors::setGyroScale(float scale) {
00101     g_scale[_z_] = scale;
00102 }
00103 
00104 
00105 /* Compass_Calibrate
00106  *
00107  * Set the offset and scale calibration for the compass
00108  */
00109 void Sensors::Compass_Calibrate(float offset[3], float scale[3])
00110 {
00111     for (int i=0; i < 3; i++) {
00112         m_offset[i] = offset[i];
00113         m_scale[i] = scale[i];
00114     }    
00115  
00116     return;
00117 }
00118 
00119 void Sensors::configureEncoders(float tireCirc, int encStripes)
00120 {
00121     _tireCirc = tireCirc;
00122     _encStripes = encStripes;
00123 }
00124 
00125 
00126 void Sensors::Read_Encoders()
00127 {
00128     // Odometry            
00129     leftCount = _left.read();
00130     rightCount = _right.read();
00131     leftTotal += leftCount;
00132     rightTotal += rightCount;
00133 
00134     float ticksPerDist = _tireCirc / _encStripes;
00135 
00136     // TODO 3 sanity check on encoders; if difference between them
00137     //  is huge, what do we do?  Slipping wheel?  Skidding wheel?  Broken encoder?
00138     //  front encoders would be ideal as additional sanity check
00139     
00140     // TODO 3 get rid of state variable
00141     lrEncDistance  = ticksPerDist * (double) leftCount;
00142     rrEncDistance = ticksPerDist * (double) rightCount;
00143     //encDistance = (lrEncDistance + rrEncDistance) / 2.0;
00144     // compute speed from time between ticks
00145     int leftTime = _left.readTime();
00146     int rightTime = _right.readTime();
00147 
00148     // We could also supplement this with distance/time calcs once we get up to a higher
00149     // speed and encoder quantization error is lower
00150     // To reduce asymmetry of the encoder operation, we're only reading time
00151     // between rising ticks. So that means half the wheel stripes
00152 
00153     if (leftTime <= 0) {
00154         lrEncSpeed = 0;
00155     } else {
00156         lrEncSpeed = (2.0 * ticksPerDist) / ((float) leftTime * 1e-6);
00157     }
00158     
00159     if (rightTime <= 0) {
00160         rrEncSpeed = 0;
00161     } else {
00162         rrEncSpeed = (2.0 * ticksPerDist) / ((float) rightTime * 1e-6);
00163     }
00164         
00165     // Dead band
00166     if (lrEncSpeed < 0.1) lrEncSpeed = 0;
00167     if (rrEncSpeed < 0.1) rrEncSpeed = 0;
00168     // TODO: 3 use more sophisticated approach where we count if any ticks have happened lately
00169     // and if not, reset the speed
00170     encSpeed = (lrEncSpeed + rrEncSpeed) / 2.0;
00171 }
00172 
00173 
00174 void Sensors::Read_Gyro()
00175 {
00176     _gyro.read(g);
00177     gTemp = (int) _gyro.readTemp();
00178     
00179     gyro[_x_] = g_sign[_x_] * (g[_x_] - g_offset[_x_]); // really need to scale this too
00180     gyro[_y_] = g_sign[_y_] * (g[_y_] - g_offset[_y_]); // really need to scale this too
00181     gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
00182 
00183     return;
00184 }
00185 
00186 // Reads x,y and z accelerometer registers
00187 void Sensors::Read_Accel()
00188 {
00189     _compass.readAcc(a);
00190 
00191     accel[_x_] = a_sign[_x_] * (a[_x_] - a_offset[_x_]);
00192     accel[_y_] = a_sign[_y_] * (a[_y_] - a_offset[_y_]);
00193     accel[_z_] = a_sign[_z_] * (a[_z_] - a_offset[_z_]);
00194 
00195     return;  
00196 }
00197 
00198 void Sensors::Read_Compass()
00199 {
00200     _compass.readMag(m);
00201     // adjust for compass axis offsets, scale, and orientation (sign)
00202     for (int i=0; i < 3; i++) {
00203         mag[i] = ((float) m[i] - m_offset[i]) * m_scale[i] * m_sign[i];
00204     }
00205     //fprintf(stdout, "m_offset=(%5.5f,%5.5f,%5.5f)\n", ahrs.c_magnetom[0],ahrs.c_magnetom[1],ahrs.c_magnetom[2]);
00206   
00207     return;
00208 }
00209 
00210 /* doing some crude gryo and accelerometer offset calibration here
00211  *
00212  */
00213 void Sensors::Calculate_Offsets()
00214 {
00215     int samples=128;
00216     
00217     for (int i=0; i < 3; i++) {
00218         g_offset[i] = 0;
00219         a_offset[i] = 0;
00220     }
00221 
00222     for(int i=0; i < samples; i++) {  // We take some readings...
00223         Read_Gyro();
00224         Read_Accel();
00225         wait(0.010); // sample at ~100hz
00226         for(int y=0; y < 3; y++) {   // accumulate values
00227             g_offset[y] += g[y];
00228             a_offset[y] += a[y];
00229         }
00230     }
00231 
00232     for(int y=0; y < 3; y++) {
00233         g_offset[y] /= samples;
00234         a_offset[y] /= samples;
00235     }
00236 
00237     //TODO 4 if we ever get back to using accelerometer, do something about this.
00238     //a_offset[_z_] -= GRAVITY * a_sign[_z_]; // I don't get why we're doing this...?
00239 }
00240 
00241 
00242 
00243 void Sensors::Compass_Heading()
00244 {
00245   /*
00246   float MAG_X;
00247   float MAG_Y;
00248   float cos_roll;
00249   float sin_roll;
00250   float cos_pitch;
00251   float sin_pitch;
00252   
00253   // See DCM.cpp Drift_correction()
00254 
00255   // umm... doesn't the dcm already have this info in it?!
00256   cos_roll = cos(ahrs.roll);
00257   sin_roll = sin(ahrs.roll);
00258   cos_pitch = cos(ahrs.pitch);
00259   sin_pitch = sin(ahrs.pitch);
00260     
00261   // Tilt compensated Magnetic filed X:
00262   MAG_X = mag[_x_] * cos_pitch + mag[_y_] * sin_roll * sin_pitch + mag[_z_] * cos_roll * sin_pitch;
00263   // Tilt compensated Magnetic filed Y:
00264   MAG_Y = mag[_y_] * cos_roll - mag[_z_] * sin_roll;
00265   // Magnetic Heading
00266   ahrs.MAG_Heading = atan2(-MAG_Y,MAG_X);
00267   */
00268 }
00269 
00270 
00271 void Sensors::getRawMag(int rawmag[3])
00272 {
00273     Read_Compass();
00274     for (int i=0; i < 3; i++) {
00275         rawmag[i] = m[i];
00276     }        
00277 }
00278 
00279 
00280 // getCurrent is a macro defined above
00281 
00282 float Sensors::getVoltage() {
00283     static float filter = -1.0;
00284     float v = _voltage * 3.3 * 4.12712;        // 242.3mV/V
00285 
00286     // TODO 3 median filter to eliminate spikes
00287     if (filter < 0) {
00288         filter = v * VFF;
00289     } else {
00290         filter += (v - filter/VFF);
00291     }
00292     return (filter / VFF);
00293 }
00294 
00295 
00296 
00297 float Sensors::getCurrent() {
00298     /*static bool first=true;
00299     static float history[3];
00300     float tmp;
00301     float sort[3];
00302 
00303     if (first) {
00304         first = false;
00305         history[0] = history[1] = history[2] = _current;
00306     } else {
00307         sort[0] = history[0] = history[1];
00308         sort[1] = history[1] = history[2];
00309         sort[2] = history[2] = _current;
00310     }
00311     BubbleSort(sort, 3);*/
00312     return (_current * 3.3 * 13.6612); // 73.20mV/A        
00313 }
00314 
00315 
00316 void Sensors::Read_Power()
00317 {
00318     // TODO 3 exponential or median filtering on these to get rid of spikes
00319     //
00320     voltage = getVoltage();
00321     current = getCurrent();
00322 
00323     return;
00324 }
00325 
00326 float clampIRRanger(float x)
00327 {
00328     float y=x;
00329     
00330     if (y < 0.1) 
00331         y = 0.1;
00332     if (y > 5.0) 
00333         y = 5.0;
00334     
00335     return y;
00336 }
00337 
00338 void Sensors::Read_Camera()
00339 {
00340     char count;
00341     //char data[128];
00342     //char addr=0x80;
00343     /*
00344     struct {
00345         int color;
00346         int x1;
00347         int y1;
00348         int x2;
00349         int y2;
00350     } blob;
00351     */
00352 
00353     /* 
00354     I2C START BIT
00355     WRITE: 0xA0 ACK 
00356     WRITE: 0x00 ACK 
00357     I2C START BIT
00358     WRITE: 0xA1 ACK 
00359     READ: 0x01  ACK 0x02  ACK 0x03  ACK 0x04  ACK 0x05  ACK 0x06  ACK 0x07  ACK 0x08  ACK 0x09  ACK 0x0A 
00360     NACK
00361     I2C STOP BIT
00362     */
00363 
00364     _cam.start();
00365     _cam.write(0x11<<1);
00366     _cam.write(0x01); // command to send box data
00367     _cam.start();
00368     _cam.write((0x11<<1 | 0x01));
00369     count = _cam.read(1); // number of boxes tracked
00370     if (count > 8) count = 8;
00371     //fprintf(stdout, "----------\n%d\n", count);
00372     int x=0;
00373     for (int i=0; i < count; i++) {
00374         //fprintf(stdout, "%d: ", i);
00375         for (int j=0; j < 5; j++) {
00376             //data[x] = _cam.read(1);
00377             //fprintf(stdout, "%d ", data[x]);
00378             x++;
00379         }
00380         //fprintf(stdout, "\n");
00381     }
00382     _cam.read(0); // easiest to just read one more byte then NACK it
00383     _cam.stop();
00384 }
00385 
00386 
00387 void Sensors::Read_Rangers()
00388 {
00389     char data[2];
00390     // Sharp GP2Y0A710YK0F
00391     static float m=288.12;  // slope adc=m(dist) + b
00392     static float b=219.3;
00393     static float m_per_lsb=0.004883/0.38583; // 0.004883mV/lsb * 0.0098in/lsb * 1in/0.0254m = m/lsb
00394     
00395     _rangers.start();
00396     data[0] = (0x11<<1 | 0x01); // send address + read = 1
00397     _rangers.write(data[0]);       // send address
00398     ranger[0] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8;
00399     ranger[1] = (int) _rangers.read(1) | ((int) _rangers.read(1)) << 8;
00400     ranger[2] = (int) _rangers.read(1) | ((int) _rangers.read(0)) << 8; // don't ack the last byte
00401     _rangers.stop();
00402 
00403     /*
00404     for (int q=0; q<3; q++)
00405         fprintf(stdout, "%04x ", ranger[q]);    
00406     fprintf(stdout, "\n");
00407     */
00408     
00409     if (ranger[0] < 86) {
00410         rightRanger = 999.0;
00411     } else if (ranger[0] > 585) {
00412         rightRanger = 20.0;
00413     } else {
00414         // Sharp GP2Y0A02YK0F
00415         rightRanger = 1/(8.664e-005*ranger[0]-0.0008); // IR distance, meters
00416     }
00417 
00418     // Compute distances
00419     leftRanger = clampIRRanger( m/(ranger[1]-b) );  // IR distance, meters
00420     // Sonar
00421     centerRanger = ranger[2] * (m_per_lsb); // Sonar distance, metersmv/in=0.0098, mv/lsb=0.0049
00422 }
00423 
00424 
00425 
00426 /** perform bubble sort
00427  * for median filtering
00428  */
00429 void Sensors::BubbleSort(float *num, int numLength)
00430 {
00431     float temp;             // holding variable
00432     bool flag=true;
00433     
00434     for(int i = 1; (i <= numLength) && flag; i++) {
00435         flag = false;
00436         for (int j = 0; j < (numLength -1); j++) {
00437             if (num[j+1] > num[j]) {      // ascending order simply changes to <
00438                 temp = num[j];             // swap elements
00439                 num[j] = num[j+1];
00440                 num[j+1] = temp;
00441                 flag = true;
00442             }
00443         }
00444     }
00445     return;   //arrays are passed to functions by address; nothing is returned
00446 }