Michael Shimniok / Mbed 2 deprecated DataBus

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@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
12:5dfa1ab47838 
Initial publish of revised version.

Who changed what in which revision?

UserRevisionLine numberNew 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 0:a6a169de725f 35 #include "Ublox6.h"
shimniok 0:a6a169de725f 36 #include "L3G4200D.h"
shimniok 0:a6a169de725f 37 #include "LSM303DLM.h"
shimniok 0:a6a169de725f 38 #include "IncrementalEncoder.h"
shimniok 0:a6a169de725f 39 #include "Matrix.h"
shimniok 0:a6a169de725f 40
shimniok 0:a6a169de725f 41 // Sensor axes
shimniok 0:a6a169de725f 42 #define _x_ 0
shimniok 0:a6a169de725f 43 #define _y_ 1
shimniok 0:a6a169de725f 44 #define _z_ 2
shimniok 0:a6a169de725f 45
shimniok 0:a6a169de725f 46 // Magnetometer calibration constants
shimniok 0:a6a169de725f 47 //#define M_OFFSET_X -20.4416 // calibrated 02/13/2012 mes
shimniok 0:a6a169de725f 48 //#define M_OFFSET_Y -67.2318
shimniok 0:a6a169de725f 49 //#define M_OFFSET_Z 6.0950
shimniok 0:a6a169de725f 50 //#define M_OFFSET_X 12.8922 // calibrated 12/31/2012 mes
shimniok 0:a6a169de725f 51 //#define M_OFFSET_Y -7.3453
shimniok 0:a6a169de725f 52 //#define M_OFFSET_Z -147.8652
shimniok 0:a6a169de725f 53 //#define M_SCALE_X 570.06
shimniok 0:a6a169de725f 54 //#define M_SCALE_Y 532.83
shimniok 0:a6a169de725f 55 //#define M_SCALE_Z 480.95
shimniok 0:a6a169de725f 56
shimniok 0:a6a169de725f 57 //#define M_X_MIN -654 // calibrated 12/8/2011 mes
shimniok 0:a6a169de725f 58 //#define M_X_MAX 457
shimniok 0:a6a169de725f 59 //#define M_Y_MIN -744
shimniok 0:a6a169de725f 60 //#define M_Y_MAX 369
shimniok 0:a6a169de725f 61 //#define M_Z_MIN -573
shimniok 0:a6a169de725f 62 //#define M_Z_MAX 464
shimniok 0:a6a169de725f 63
shimniok 0:a6a169de725f 64 class Sensors {
shimniok 0:a6a169de725f 65 public:
shimniok 0:a6a169de725f 66 Sensors(void);
shimniok 0:a6a169de725f 67 void Compass_Calibrate(float offset[3], float scale[3]);
shimniok 12:5dfa1ab47838 68 void configureEncoders(float tireCirc, int encStripes);
shimniok 12:5dfa1ab47838 69 void setGyroScale(float scale);
shimniok 0:a6a169de725f 70 void Read_Encoders(void);
shimniok 0:a6a169de725f 71 void Read_Gyro(void);
shimniok 0:a6a169de725f 72 void Read_Accel(void);
shimniok 0:a6a169de725f 73 void Read_Compass(void);
shimniok 0:a6a169de725f 74 void Calculate_Offsets(void);
shimniok 0:a6a169de725f 75 void Compass_Heading(void);
shimniok 0:a6a169de725f 76 void getRawMag(int mag[3]);
shimniok 0:a6a169de725f 77 float getVoltage(void);
shimniok 0:a6a169de725f 78 float getCurrent(void);
shimniok 0:a6a169de725f 79 void Read_Power(void);
shimniok 0:a6a169de725f 80 void Read_Rangers();
shimniok 0:a6a169de725f 81 void Read_Camera();
shimniok 0:a6a169de725f 82 int g[3]; // raw gyro value
shimniok 0:a6a169de725f 83 int gTemp; // raw gyro temperature
shimniok 0:a6a169de725f 84 int a[3]; // raw accelerometer value
shimniok 0:a6a169de725f 85 int m[3]; // raw magnetometer value
shimniok 0:a6a169de725f 86 int g_offset[3]; // raw gyro offset
shimniok 0:a6a169de725f 87 int a_offset[3]; // raw accel offset
shimniok 0:a6a169de725f 88 float m_offset[3]; // magnetometer offset
shimniok 0:a6a169de725f 89 float g_scale[3]; // gyro scaling factor
shimniok 0:a6a169de725f 90 float m_scale[3]; // magnetometer scale
shimniok 0:a6a169de725f 91 int g_sign[3]; // correct sensor signs
shimniok 0:a6a169de725f 92 int a_sign[3];
shimniok 0:a6a169de725f 93 int m_sign[3];
shimniok 0:a6a169de725f 94 float gyro[3]; // corrected gyro value
shimniok 0:a6a169de725f 95 float accel[3]; // corrected accelerometer value
shimniok 0:a6a169de725f 96 float mag[3]; // corrected magnetometer value
shimniok 0:a6a169de725f 97 int ranger[3]; // ranger values
shimniok 0:a6a169de725f 98 float leftRanger;
shimniok 0:a6a169de725f 99 float rightRanger;
shimniok 0:a6a169de725f 100 float centerRanger;
shimniok 0:a6a169de725f 101 float voltage; // battery voltage in volts
shimniok 0:a6a169de725f 102 float current; // system current draw in amps
shimniok 0:a6a169de725f 103 unsigned int leftTotal; // total number of ticks
shimniok 0:a6a169de725f 104 unsigned int rightTotal; // total number of ticks
shimniok 0:a6a169de725f 105 unsigned int leftCount; // left rear encoder count
shimniok 0:a6a169de725f 106 unsigned int rightCount; // right rear encoder count
shimniok 0:a6a169de725f 107 float lrEncDistance; // left rear encoder distance
shimniok 0:a6a169de725f 108 float rrEncDistance; // right rear encoder distance
shimniok 0:a6a169de725f 109 float lrEncSpeed; // left rear encoder speed
shimniok 0:a6a169de725f 110 float rrEncSpeed; // right rear encoder speed
shimniok 0:a6a169de725f 111 float encDistance; // encoder distance since last check
shimniok 0:a6a169de725f 112 float encSpeed; // encoder calculated speed
shimniok 2:fbc6e3cf3ed8 113 //Sirf3 gps; // Pharos SiRF III GPS
shimniok 2:fbc6e3cf3ed8 114 //Venus638flpx gps; // Venus GPS
shimniok 2:fbc6e3cf3ed8 115 Ublox6 gps; // Ublox6 GPS
shimniok 0:a6a169de725f 116 AnalogIn _voltage; // Voltage from sensor board
shimniok 0:a6a169de725f 117 AnalogIn _current; // Current from sensor board
shimniok 0:a6a169de725f 118 IncrementalEncoder _left; // Left wheel encoder
shimniok 0:a6a169de725f 119 IncrementalEncoder _right; // Right wheel encoder
shimniok 0:a6a169de725f 120 L3G4200D _gyro; // MinIMU-9 gyro
shimniok 0:a6a169de725f 121 LSM303DLM _compass; // MinIMU-9 compass/accelerometer
shimniok 0:a6a169de725f 122 I2C _rangers; // Arduino ranger board
shimniok 0:a6a169de725f 123 I2C _cam; // Propeller camer aboard
shimniok 0:a6a169de725f 124 //HMC5843 compass;
shimniok 0:a6a169de725f 125
shimniok 0:a6a169de725f 126 private:
shimniok 0:a6a169de725f 127 void BubbleSort(float *num, int numLength);
shimniok 12:5dfa1ab47838 128 float _tireCirc; // tire circumference
shimniok 12:5dfa1ab47838 129 int _encStripes; // encoder ticks per revolution
shimniok 0:a6a169de725f 130 };
shimniok 0:a6a169de725f 131
shimniok 12:5dfa1ab47838 132 #endif