Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Sensors/Sensors.h@2:fbc6e3cf3ed8, 2013-06-06 (annotated)
- Committer:
- shimniok
- Date:
- Thu Jun 06 13:40:23 2013 +0000
- Revision:
- 2:fbc6e3cf3ed8
- Parent:
- 0:a6a169de725f
- Child:
- 12:5dfa1ab47838
Sort-of working version, still some errors with estimation. Added clamp() function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:a6a169de725f | 1 | /* |
shimniok | 0:a6a169de725f | 2 | |
shimniok | 0:a6a169de725f | 3 | MinIMU-9-Arduino-AHRS |
shimniok | 0:a6a169de725f | 4 | Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) |
shimniok | 0:a6a169de725f | 5 | |
shimniok | 0:a6a169de725f | 6 | Copyright (c) 2011 Pololu Corporation. |
shimniok | 0:a6a169de725f | 7 | http://www.pololu.com/ |
shimniok | 0:a6a169de725f | 8 | |
shimniok | 0:a6a169de725f | 9 | MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: |
shimniok | 0:a6a169de725f | 10 | http://code.google.com/p/sf9domahrs/ |
shimniok | 0:a6a169de725f | 11 | |
shimniok | 0:a6a169de725f | 12 | sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose |
shimniok | 0:a6a169de725f | 13 | Julio and Doug Weibel: |
shimniok | 0:a6a169de725f | 14 | http://code.google.com/p/ardu-imu/ |
shimniok | 0:a6a169de725f | 15 | |
shimniok | 0:a6a169de725f | 16 | MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it |
shimniok | 0:a6a169de725f | 17 | under the terms of the GNU Lesser General Public License as published by the |
shimniok | 0:a6a169de725f | 18 | Free Software Foundation, either version 3 of the License, or (at your option) |
shimniok | 0:a6a169de725f | 19 | any later version. |
shimniok | 0:a6a169de725f | 20 | |
shimniok | 0:a6a169de725f | 21 | MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but |
shimniok | 0:a6a169de725f | 22 | WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
shimniok | 0:a6a169de725f | 23 | FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for |
shimniok | 0:a6a169de725f | 24 | more details. |
shimniok | 0:a6a169de725f | 25 | |
shimniok | 0:a6a169de725f | 26 | You should have received a copy of the GNU Lesser General Public License along |
shimniok | 0:a6a169de725f | 27 | with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. |
shimniok | 0:a6a169de725f | 28 | |
shimniok | 0:a6a169de725f | 29 | */ |
shimniok | 0:a6a169de725f | 30 | #ifndef __SENSORS_H |
shimniok | 0:a6a169de725f | 31 | #define __SENSORS_H |
shimniok | 0:a6a169de725f | 32 | |
shimniok | 0:a6a169de725f | 33 | /** Sensor interface library abstracts sensor drivers, next step to a pluggable architecture */ |
shimniok | 0:a6a169de725f | 34 | |
shimniok | 2:fbc6e3cf3ed8 | 35 | #include "Sirf3.h" |
shimniok | 0:a6a169de725f | 36 | #include "Ublox6.h" |
shimniok | 0:a6a169de725f | 37 | #include "Venus638flpx.h" |
shimniok | 0:a6a169de725f | 38 | #include "L3G4200D.h" |
shimniok | 0:a6a169de725f | 39 | #include "LSM303DLM.h" |
shimniok | 0:a6a169de725f | 40 | //#include "HMC5843.h" |
shimniok | 0:a6a169de725f | 41 | #include "IncrementalEncoder.h" |
shimniok | 0:a6a169de725f | 42 | #include "Matrix.h" |
shimniok | 0:a6a169de725f | 43 | |
shimniok | 0:a6a169de725f | 44 | // Sensor axes |
shimniok | 0:a6a169de725f | 45 | #define _x_ 0 |
shimniok | 0:a6a169de725f | 46 | #define _y_ 1 |
shimniok | 0:a6a169de725f | 47 | #define _z_ 2 |
shimniok | 0:a6a169de725f | 48 | |
shimniok | 0:a6a169de725f | 49 | // Magnetometer calibration constants |
shimniok | 0:a6a169de725f | 50 | //#define M_OFFSET_X -20.4416 // calibrated 02/13/2012 mes |
shimniok | 0:a6a169de725f | 51 | //#define M_OFFSET_Y -67.2318 |
shimniok | 0:a6a169de725f | 52 | //#define M_OFFSET_Z 6.0950 |
shimniok | 0:a6a169de725f | 53 | //#define M_OFFSET_X 12.8922 // calibrated 12/31/2012 mes |
shimniok | 0:a6a169de725f | 54 | //#define M_OFFSET_Y -7.3453 |
shimniok | 0:a6a169de725f | 55 | //#define M_OFFSET_Z -147.8652 |
shimniok | 0:a6a169de725f | 56 | //#define M_SCALE_X 570.06 |
shimniok | 0:a6a169de725f | 57 | //#define M_SCALE_Y 532.83 |
shimniok | 0:a6a169de725f | 58 | //#define M_SCALE_Z 480.95 |
shimniok | 0:a6a169de725f | 59 | |
shimniok | 0:a6a169de725f | 60 | //#define M_X_MIN -654 // calibrated 12/8/2011 mes |
shimniok | 0:a6a169de725f | 61 | //#define M_X_MAX 457 |
shimniok | 0:a6a169de725f | 62 | //#define M_Y_MIN -744 |
shimniok | 0:a6a169de725f | 63 | //#define M_Y_MAX 369 |
shimniok | 0:a6a169de725f | 64 | //#define M_Z_MIN -573 |
shimniok | 0:a6a169de725f | 65 | //#define M_Z_MAX 464 |
shimniok | 0:a6a169de725f | 66 | |
shimniok | 0:a6a169de725f | 67 | // Chassis specific parameters |
shimniok | 0:a6a169de725f | 68 | #define WHEEL_STRIPES 32 |
shimniok | 0:a6a169de725f | 69 | #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Wheel circumference measured 13.125" or 0.333375m |
shimniok | 0:a6a169de725f | 70 | #define WHEELBASE 0.290 |
shimniok | 0:a6a169de725f | 71 | #define TRACK 0.280 |
shimniok | 0:a6a169de725f | 72 | |
shimniok | 0:a6a169de725f | 73 | class Sensors { |
shimniok | 0:a6a169de725f | 74 | public: |
shimniok | 0:a6a169de725f | 75 | Sensors(void); |
shimniok | 0:a6a169de725f | 76 | void Compass_Calibrate(float offset[3], float scale[3]); |
shimniok | 0:a6a169de725f | 77 | void Read_Encoders(void); |
shimniok | 0:a6a169de725f | 78 | void Read_Gyro(void); |
shimniok | 0:a6a169de725f | 79 | void Read_Accel(void); |
shimniok | 0:a6a169de725f | 80 | void Read_Compass(void); |
shimniok | 0:a6a169de725f | 81 | void Calculate_Offsets(void); |
shimniok | 0:a6a169de725f | 82 | void Compass_Heading(void); |
shimniok | 0:a6a169de725f | 83 | void getRawMag(int mag[3]); |
shimniok | 0:a6a169de725f | 84 | float getVoltage(void); |
shimniok | 0:a6a169de725f | 85 | float getCurrent(void); |
shimniok | 0:a6a169de725f | 86 | void Read_Power(void); |
shimniok | 0:a6a169de725f | 87 | void Read_Rangers(); |
shimniok | 0:a6a169de725f | 88 | void Read_Camera(); |
shimniok | 0:a6a169de725f | 89 | int g[3]; // raw gyro value |
shimniok | 0:a6a169de725f | 90 | int gTemp; // raw gyro temperature |
shimniok | 0:a6a169de725f | 91 | int a[3]; // raw accelerometer value |
shimniok | 0:a6a169de725f | 92 | int m[3]; // raw magnetometer value |
shimniok | 0:a6a169de725f | 93 | int g_offset[3]; // raw gyro offset |
shimniok | 0:a6a169de725f | 94 | int a_offset[3]; // raw accel offset |
shimniok | 0:a6a169de725f | 95 | float m_offset[3]; // magnetometer offset |
shimniok | 0:a6a169de725f | 96 | float g_scale[3]; // gyro scaling factor |
shimniok | 0:a6a169de725f | 97 | float m_scale[3]; // magnetometer scale |
shimniok | 0:a6a169de725f | 98 | int g_sign[3]; // correct sensor signs |
shimniok | 0:a6a169de725f | 99 | int a_sign[3]; |
shimniok | 0:a6a169de725f | 100 | int m_sign[3]; |
shimniok | 0:a6a169de725f | 101 | float gyro[3]; // corrected gyro value |
shimniok | 0:a6a169de725f | 102 | float accel[3]; // corrected accelerometer value |
shimniok | 0:a6a169de725f | 103 | float mag[3]; // corrected magnetometer value |
shimniok | 0:a6a169de725f | 104 | int ranger[3]; // ranger values |
shimniok | 0:a6a169de725f | 105 | float leftRanger; |
shimniok | 0:a6a169de725f | 106 | float rightRanger; |
shimniok | 0:a6a169de725f | 107 | float centerRanger; |
shimniok | 0:a6a169de725f | 108 | float voltage; // battery voltage in volts |
shimniok | 0:a6a169de725f | 109 | float current; // system current draw in amps |
shimniok | 0:a6a169de725f | 110 | unsigned int leftTotal; // total number of ticks |
shimniok | 0:a6a169de725f | 111 | unsigned int rightTotal; // total number of ticks |
shimniok | 0:a6a169de725f | 112 | unsigned int leftCount; // left rear encoder count |
shimniok | 0:a6a169de725f | 113 | unsigned int rightCount; // right rear encoder count |
shimniok | 0:a6a169de725f | 114 | float lrEncDistance; // left rear encoder distance |
shimniok | 0:a6a169de725f | 115 | float rrEncDistance; // right rear encoder distance |
shimniok | 0:a6a169de725f | 116 | float lrEncSpeed; // left rear encoder speed |
shimniok | 0:a6a169de725f | 117 | float rrEncSpeed; // right rear encoder speed |
shimniok | 0:a6a169de725f | 118 | float encDistance; // encoder distance since last check |
shimniok | 0:a6a169de725f | 119 | float encSpeed; // encoder calculated speed |
shimniok | 0:a6a169de725f | 120 | |
shimniok | 2:fbc6e3cf3ed8 | 121 | //Sirf3 gps; // Pharos SiRF III GPS |
shimniok | 2:fbc6e3cf3ed8 | 122 | //Venus638flpx gps; // Venus GPS |
shimniok | 2:fbc6e3cf3ed8 | 123 | Ublox6 gps; // Ublox6 GPS |
shimniok | 0:a6a169de725f | 124 | AnalogIn _voltage; // Voltage from sensor board |
shimniok | 0:a6a169de725f | 125 | AnalogIn _current; // Current from sensor board |
shimniok | 0:a6a169de725f | 126 | IncrementalEncoder _left; // Left wheel encoder |
shimniok | 0:a6a169de725f | 127 | IncrementalEncoder _right; // Right wheel encoder |
shimniok | 0:a6a169de725f | 128 | L3G4200D _gyro; // MinIMU-9 gyro |
shimniok | 0:a6a169de725f | 129 | LSM303DLM _compass; // MinIMU-9 compass/accelerometer |
shimniok | 0:a6a169de725f | 130 | I2C _rangers; // Arduino ranger board |
shimniok | 0:a6a169de725f | 131 | I2C _cam; // Propeller camer aboard |
shimniok | 0:a6a169de725f | 132 | //HMC5843 compass; |
shimniok | 0:a6a169de725f | 133 | |
shimniok | 0:a6a169de725f | 134 | private: |
shimniok | 0:a6a169de725f | 135 | void BubbleSort(float *num, int numLength); |
shimniok | 0:a6a169de725f | 136 | }; |
shimniok | 0:a6a169de725f | 137 | |
shimniok | 0:a6a169de725f | 138 | #endif |