Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Sensors/Sensors.h@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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