Sensors.h

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 #ifndef __SENSORS_H
00031 #define __SENSORS_H
00032 
00033 /** Sensor interface library abstracts sensor drivers, next step to a pluggable architecture */
00034 
00035 #include "L3G4200D.h"
00036 #include "LSM303DLM.h"
00037 //#include "HMC5843.h"
00038 #include "IncrementalEncoder.h"
00039 #include "Matrix.h"
00040 
00041 // Sensor axes
00042 #define _x_ 0
00043 #define _y_ 1
00044 #define _z_ 2
00045 
00046 // Magnetometer calibration constants
00047 //#define M_OFFSET_X -20.4416 // calibrated 02/13/2012 mes
00048 //#define M_OFFSET_Y -67.2318
00049 //#define M_OFFSET_Z 6.0950
00050 //#define M_OFFSET_X 12.8922 // calibrated 12/31/2012 mes
00051 //#define M_OFFSET_Y -7.3453
00052 //#define M_OFFSET_Z -147.8652
00053 //#define M_SCALE_X 570.06
00054 //#define M_SCALE_Y 532.83
00055 //#define M_SCALE_Z 480.95
00056 
00057 //#define M_X_MIN -654 // calibrated 12/8/2011 mes
00058 //#define M_X_MAX 457
00059 //#define M_Y_MIN -744 
00060 //#define M_Y_MAX 369
00061 //#define M_Z_MIN -573
00062 //#define M_Z_MAX 464
00063 
00064 // Chassis specific parameters
00065 #define WHEEL_STRIPES 32
00066 #define WHEEL_CIRC    0.321537 // m; calibrated with 4 12.236m runs. Wheel circumference measured 13.125" or 0.333375m
00067 #define WHEELBASE     0.290
00068 #define TRACK         0.280
00069 
00070 class Sensors {
00071 public:
00072     Sensors(void);
00073     void Compass_Calibrate(float offset[3], float scale[3]);
00074     void Read_Encoders(void);
00075     void Read_Gyro(void);
00076     void Read_Accel(void);
00077     void Read_Compass(void);
00078     void Calculate_Offsets(void);
00079     void Compass_Heading(void);
00080     void getRawMag(int mag[3]);
00081     float getVoltage(void);
00082     float getCurrent(void);
00083     void Read_Power(void);
00084     void Read_Rangers();
00085     void Read_Camera();
00086     int g[3];                               // raw gyro value
00087     int gTemp;                              // raw gyro temperature
00088     int a[3];                               // raw accelerometer value
00089     int m[3];                               // raw magnetometer value
00090     int g_offset[3];                        // raw gyro offset
00091     int a_offset[3];                        // raw accel offset
00092     float m_offset[3];                      // magnetometer offset
00093     float g_scale[3];                       // gyro scaling factor
00094     float m_scale[3];                       // magnetometer scale
00095     int g_sign[3];                          // correct sensor signs
00096     int a_sign[3];
00097     int m_sign[3]; 
00098     float gyro[3];                          // corrected gyro value
00099     float accel[3];                         // corrected accelerometer value
00100     float mag[3];                           // corrected magnetometer value
00101     int ranger[3];                          // ranger values
00102     float leftRanger;
00103     float rightRanger;
00104     float centerRanger;
00105     float voltage;                          // battery voltage in volts
00106     float current;                          // system current draw in amps
00107     unsigned int leftTotal;                 // total number of ticks
00108     unsigned int rightTotal;                // total number of ticks
00109     unsigned int leftCount;                 // left rear encoder count
00110     unsigned int rightCount;                // right rear encoder count
00111     float lrEncDistance;                    // left rear encoder distance
00112     float rrEncDistance;                    // right rear encoder distance
00113     float lrEncSpeed;                       // left rear encoder speed
00114     float rrEncSpeed;                       // right rear encoder speed
00115     float encDistance;                      // encoder distance since last check
00116     float encSpeed;                         // encoder calculated speed
00117 
00118     AnalogIn _voltage;                      // Voltage from sensor board
00119     AnalogIn _current;                      // Current from sensor board
00120     IncrementalEncoder _left;               // Left wheel encoder
00121     IncrementalEncoder _right;              // Right wheel encoder
00122     L3G4200D _gyro;                         // MinIMU-9 gyro
00123     LSM303DLM _compass;                     // MinIMU-9 compass/accelerometer
00124     I2C _rangers;                           // Arduino ranger board
00125     I2C _cam;                               // Propeller camer aboard
00126     //HMC5843 compass;
00127 
00128 private:
00129     void BubbleSort(float *num, int numLength);
00130 };
00131 
00132 #endif