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 }