Isabella Gomez Torres / Mbed OS TEST_SideToF

Where we will test the side ToF sensors

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchairControlSideTof/IMU6050/MPU6050.h@21:d1faccb96146, 2019-07-09 (annotated)

Committer:
isagmz
Date:
Tue Jul 09 17:52:32 2019 +0000
Revision:
21:d1faccb96146
Finished IMU side sensor code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
isagmz 21:d1faccb96146 1 /*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output.
isagmz 21:d1faccb96146 2 If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
isagmz 21:d1faccb96146 3 */
isagmz 21:d1faccb96146 4
isagmz 21:d1faccb96146 5
isagmz 21:d1faccb96146 6 #ifndef MPU6050_H
isagmz 21:d1faccb96146 7 #define MPU6050_H
isagmz 21:d1faccb96146 8
isagmz 21:d1faccb96146 9 /**
isagmz 21:d1faccb96146 10 * Includes
isagmz 21:d1faccb96146 11 */
isagmz 21:d1faccb96146 12 #include "mbed.h"
isagmz 21:d1faccb96146 13
isagmz 21:d1faccb96146 14
isagmz 21:d1faccb96146 15 /**
isagmz 21:d1faccb96146 16 * Defines
isagmz 21:d1faccb96146 17 */
isagmz 21:d1faccb96146 18 #ifndef MPU6050_ADDRESS
isagmz 21:d1faccb96146 19 #define MPU6050_ADDRESS 0x68 // address pin low (GND), default for InvenSense evaluation board
isagmz 21:d1faccb96146 20 #endif
isagmz 21:d1faccb96146 21
isagmz 21:d1faccb96146 22 #ifdef MPU6050_ES
isagmz 21:d1faccb96146 23 #define DOUBLE_ACCELERO
isagmz 21:d1faccb96146 24 #endif
isagmz 21:d1faccb96146 25
isagmz 21:d1faccb96146 26 /**
isagmz 21:d1faccb96146 27 * Registers
isagmz 21:d1faccb96146 28 */
isagmz 21:d1faccb96146 29 #define MPU6050_CONFIG_REG 0x1A
isagmz 21:d1faccb96146 30 #define MPU6050_GYRO_CONFIG_REG 0x1B
isagmz 21:d1faccb96146 31 #define MPU6050_ACCELERO_CONFIG_REG 0x1C
isagmz 21:d1faccb96146 32
isagmz 21:d1faccb96146 33 #define MPU6050_INT_PIN_CFG 0x37
isagmz 21:d1faccb96146 34
isagmz 21:d1faccb96146 35 #define MPU6050_ACCEL_XOUT_H_REG 0x3B
isagmz 21:d1faccb96146 36 #define MPU6050_ACCEL_YOUT_H_REG 0x3D
isagmz 21:d1faccb96146 37 #define MPU6050_ACCEL_ZOUT_H_REG 0x3F
isagmz 21:d1faccb96146 38
isagmz 21:d1faccb96146 39 #define MPU6050_TEMP_H_REG 0x41
isagmz 21:d1faccb96146 40
isagmz 21:d1faccb96146 41 #define MPU6050_GYRO_XOUT_H_REG 0x43
isagmz 21:d1faccb96146 42 #define MPU6050_GYRO_YOUT_H_REG 0x45
isagmz 21:d1faccb96146 43 #define MPU6050_GYRO_ZOUT_H_REG 0x47
isagmz 21:d1faccb96146 44
isagmz 21:d1faccb96146 45
isagmz 21:d1faccb96146 46
isagmz 21:d1faccb96146 47 #define MPU6050_PWR_MGMT_1_REG 0x6B
isagmz 21:d1faccb96146 48 #define MPU6050_WHO_AM_I_REG 0x75
isagmz 21:d1faccb96146 49
isagmz 21:d1faccb96146 50
isagmz 21:d1faccb96146 51
isagmz 21:d1faccb96146 52 /**
isagmz 21:d1faccb96146 53 * Definitions
isagmz 21:d1faccb96146 54 */
isagmz 21:d1faccb96146 55 #define MPU6050_SLP_BIT 6
isagmz 21:d1faccb96146 56 #define MPU6050_BYPASS_BIT 1
isagmz 21:d1faccb96146 57
isagmz 21:d1faccb96146 58 #define MPU6050_BW_256 0
isagmz 21:d1faccb96146 59 #define MPU6050_BW_188 1
isagmz 21:d1faccb96146 60 #define MPU6050_BW_98 2
isagmz 21:d1faccb96146 61 #define MPU6050_BW_42 3
isagmz 21:d1faccb96146 62 #define MPU6050_BW_20 4
isagmz 21:d1faccb96146 63 #define MPU6050_BW_10 5
isagmz 21:d1faccb96146 64 #define MPU6050_BW_5 6
isagmz 21:d1faccb96146 65
isagmz 21:d1faccb96146 66 #define MPU6050_ACCELERO_RANGE_2G 0
isagmz 21:d1faccb96146 67 #define MPU6050_ACCELERO_RANGE_4G 1
isagmz 21:d1faccb96146 68 #define MPU6050_ACCELERO_RANGE_8G 2
isagmz 21:d1faccb96146 69 #define MPU6050_ACCELERO_RANGE_16G 3
isagmz 21:d1faccb96146 70
isagmz 21:d1faccb96146 71 #define MPU6050_GYRO_RANGE_250 0
isagmz 21:d1faccb96146 72 #define MPU6050_GYRO_RANGE_500 1
isagmz 21:d1faccb96146 73 #define MPU6050_GYRO_RANGE_1000 2
isagmz 21:d1faccb96146 74 #define MPU6050_GYRO_RANGE_2000 3
isagmz 21:d1faccb96146 75
isagmz 21:d1faccb96146 76 //interrupt address
isagmz 21:d1faccb96146 77 #define MPU6050_RA_INT_ENABLE 0x38
isagmz 21:d1faccb96146 78 //define how the accelerometer is placed on surface
isagmz 21:d1faccb96146 79 #define X_AXIS 1
isagmz 21:d1faccb96146 80 #define Y_AXIS 2
isagmz 21:d1faccb96146 81 #define Z_AXIS 0
isagmz 21:d1faccb96146 82 //translation from radians to degrees
isagmz 21:d1faccb96146 83 #define RADIANS_TO_DEGREES 180/3.1415926536
isagmz 21:d1faccb96146 84 //constant used for the complementary filter, which ranges usually from 0.9 to 1.0
isagmz 21:d1faccb96146 85 #define ALPHA 0.96 //filter constant
isagmz 21:d1faccb96146 86 //scale the gyro
isagmz 21:d1faccb96146 87 #define GYRO_SCALE 2.7176
isagmz 21:d1faccb96146 88
isagmz 21:d1faccb96146 89 /** MPU6050 IMU library.
isagmz 21:d1faccb96146 90 *
isagmz 21:d1faccb96146 91 * Example:
isagmz 21:d1faccb96146 92 * @code
isagmz 21:d1faccb96146 93 * Later, maybe
isagmz 21:d1faccb96146 94 * @endcode
isagmz 21:d1faccb96146 95 */
isagmz 21:d1faccb96146 96 class MPU6050 {
isagmz 21:d1faccb96146 97 public:
isagmz 21:d1faccb96146 98 /**
isagmz 21:d1faccb96146 99 * Constructor.
isagmz 21:d1faccb96146 100 *
isagmz 21:d1faccb96146 101 * Sleep mode of MPU6050 is immediatly disabled
isagmz 21:d1faccb96146 102 *
isagmz 21:d1faccb96146 103 * @param sda - mbed pin to use for the SDA I2C line.
isagmz 21:d1faccb96146 104 * @param scl - mbed pin to use for the SCL I2C line.
isagmz 21:d1faccb96146 105 */
isagmz 21:d1faccb96146 106 MPU6050(PinName sda, PinName scl);
isagmz 21:d1faccb96146 107
isagmz 21:d1faccb96146 108
isagmz 21:d1faccb96146 109 /**
isagmz 21:d1faccb96146 110 * Tests the I2C connection by reading the WHO_AM_I register.
isagmz 21:d1faccb96146 111 *
isagmz 21:d1faccb96146 112 * @return True for a working connection, false for an error
isagmz 21:d1faccb96146 113 */
isagmz 21:d1faccb96146 114 bool testConnection( void );
isagmz 21:d1faccb96146 115
isagmz 21:d1faccb96146 116 /**
isagmz 21:d1faccb96146 117 * Sets the bandwidth of the digital low-pass filter
isagmz 21:d1faccb96146 118 *
isagmz 21:d1faccb96146 119 * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
isagmz 21:d1faccb96146 120 * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
isagmz 21:d1faccb96146 121 *
isagmz 21:d1faccb96146 122 * @param BW - The three bits that set the bandwidth (use the predefined macros)
isagmz 21:d1faccb96146 123 */
isagmz 21:d1faccb96146 124 void setBW( char BW );
isagmz 21:d1faccb96146 125
isagmz 21:d1faccb96146 126 /**
isagmz 21:d1faccb96146 127 * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus)
isagmz 21:d1faccb96146 128 *
isagmz 21:d1faccb96146 129 * @param state - Enables/disables the I2C bypass mode
isagmz 21:d1faccb96146 130 */
isagmz 21:d1faccb96146 131 void setI2CBypass ( bool state );
isagmz 21:d1faccb96146 132
isagmz 21:d1faccb96146 133 /**
isagmz 21:d1faccb96146 134 * Sets the Accelero full-scale range
isagmz 21:d1faccb96146 135 *
isagmz 21:d1faccb96146 136 * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
isagmz 21:d1faccb96146 137 *
isagmz 21:d1faccb96146 138 * @param range - The two bits that set the full-scale range (use the predefined macros)
isagmz 21:d1faccb96146 139 */
isagmz 21:d1faccb96146 140 void setAcceleroRange(char range);
isagmz 21:d1faccb96146 141
isagmz 21:d1faccb96146 142 /**
isagmz 21:d1faccb96146 143 * Reads the accelero x-axis.
isagmz 21:d1faccb96146 144 *
isagmz 21:d1faccb96146 145 * @return 16-bit signed integer x-axis accelero data
isagmz 21:d1faccb96146 146 */
isagmz 21:d1faccb96146 147 int getAcceleroRawX( void );
isagmz 21:d1faccb96146 148
isagmz 21:d1faccb96146 149 /**
isagmz 21:d1faccb96146 150 * Reads the accelero y-axis.
isagmz 21:d1faccb96146 151 *
isagmz 21:d1faccb96146 152 * @return 16-bit signed integer y-axis accelero data
isagmz 21:d1faccb96146 153 */
isagmz 21:d1faccb96146 154 int getAcceleroRawY( void );
isagmz 21:d1faccb96146 155
isagmz 21:d1faccb96146 156 /**
isagmz 21:d1faccb96146 157 * Reads the accelero z-axis.
isagmz 21:d1faccb96146 158 *
isagmz 21:d1faccb96146 159 * @return 16-bit signed integer z-axis accelero data
isagmz 21:d1faccb96146 160 */
isagmz 21:d1faccb96146 161 int getAcceleroRawZ( void );
isagmz 21:d1faccb96146 162
isagmz 21:d1faccb96146 163 /**
isagmz 21:d1faccb96146 164 * Reads all accelero data.
isagmz 21:d1faccb96146 165 *
isagmz 21:d1faccb96146 166 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
isagmz 21:d1faccb96146 167 */
isagmz 21:d1faccb96146 168 void getAcceleroRaw( int *data );
isagmz 21:d1faccb96146 169
isagmz 21:d1faccb96146 170 /**
isagmz 21:d1faccb96146 171 * Reads all accelero data, gives the acceleration in m/s2
isagmz 21:d1faccb96146 172 *
isagmz 21:d1faccb96146 173 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
isagmz 21:d1faccb96146 174 *
isagmz 21:d1faccb96146 175 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
isagmz 21:d1faccb96146 176 */
isagmz 21:d1faccb96146 177 void getAccelero( float *data );
isagmz 21:d1faccb96146 178
isagmz 21:d1faccb96146 179 /**
isagmz 21:d1faccb96146 180 * Sets the Gyro full-scale range
isagmz 21:d1faccb96146 181 *
isagmz 21:d1faccb96146 182 * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
isagmz 21:d1faccb96146 183 *
isagmz 21:d1faccb96146 184 * @param range - The two bits that set the full-scale range (use the predefined macros)
isagmz 21:d1faccb96146 185 */
isagmz 21:d1faccb96146 186 void setGyroRange(char range);
isagmz 21:d1faccb96146 187
isagmz 21:d1faccb96146 188 /**
isagmz 21:d1faccb96146 189 * Reads the gyro x-axis.
isagmz 21:d1faccb96146 190 *
isagmz 21:d1faccb96146 191 * @return 16-bit signed integer x-axis gyro data
isagmz 21:d1faccb96146 192 */
isagmz 21:d1faccb96146 193 int getGyroRawX( void );
isagmz 21:d1faccb96146 194
isagmz 21:d1faccb96146 195 /**
isagmz 21:d1faccb96146 196 * Reads the gyro y-axis.
isagmz 21:d1faccb96146 197 *
isagmz 21:d1faccb96146 198 * @return 16-bit signed integer y-axis gyro data
isagmz 21:d1faccb96146 199 */
isagmz 21:d1faccb96146 200 int getGyroRawY( void );
isagmz 21:d1faccb96146 201
isagmz 21:d1faccb96146 202 /**
isagmz 21:d1faccb96146 203 * Reads the gyro z-axis.
isagmz 21:d1faccb96146 204 *
isagmz 21:d1faccb96146 205 * @return 16-bit signed integer z-axis gyro data
isagmz 21:d1faccb96146 206 */
isagmz 21:d1faccb96146 207 int getGyroRawZ( void );
isagmz 21:d1faccb96146 208
isagmz 21:d1faccb96146 209 /**
isagmz 21:d1faccb96146 210 * Reads all gyro data.
isagmz 21:d1faccb96146 211 *
isagmz 21:d1faccb96146 212 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
isagmz 21:d1faccb96146 213 */
isagmz 21:d1faccb96146 214 void getGyroRaw( int *data );
isagmz 21:d1faccb96146 215
isagmz 21:d1faccb96146 216 /**
isagmz 21:d1faccb96146 217 * Reads all gyro data, gives the gyro in rad/s
isagmz 21:d1faccb96146 218 *
isagmz 21:d1faccb96146 219 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
isagmz 21:d1faccb96146 220 *
isagmz 21:d1faccb96146 221 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
isagmz 21:d1faccb96146 222 */
isagmz 21:d1faccb96146 223 void getGyro( float *data);
isagmz 21:d1faccb96146 224
isagmz 21:d1faccb96146 225 /**
isagmz 21:d1faccb96146 226 * Reads temperature data.
isagmz 21:d1faccb96146 227 *
isagmz 21:d1faccb96146 228 * @return 16 bit signed integer with the raw temperature register value
isagmz 21:d1faccb96146 229 */
isagmz 21:d1faccb96146 230 int getTempRaw( void );
isagmz 21:d1faccb96146 231
isagmz 21:d1faccb96146 232 /**
isagmz 21:d1faccb96146 233 * Returns current temperature
isagmz 21:d1faccb96146 234 *
isagmz 21:d1faccb96146 235 * @returns float with the current temperature
isagmz 21:d1faccb96146 236 */
isagmz 21:d1faccb96146 237 float getTemp( void );
isagmz 21:d1faccb96146 238
isagmz 21:d1faccb96146 239 /**
isagmz 21:d1faccb96146 240 * Sets the sleep mode of the MPU6050
isagmz 21:d1faccb96146 241 *
isagmz 21:d1faccb96146 242 * @param state - true for sleeping, false for wake up
isagmz 21:d1faccb96146 243 */
isagmz 21:d1faccb96146 244 void setSleepMode( bool state );
isagmz 21:d1faccb96146 245
isagmz 21:d1faccb96146 246
isagmz 21:d1faccb96146 247 /**
isagmz 21:d1faccb96146 248 * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU.
isagmz 21:d1faccb96146 249 *
isagmz 21:d1faccb96146 250 * @param adress - register address to write to
isagmz 21:d1faccb96146 251 * @param data - data to write
isagmz 21:d1faccb96146 252 */
isagmz 21:d1faccb96146 253 void write( char address, char data);
isagmz 21:d1faccb96146 254
isagmz 21:d1faccb96146 255 /**
isagmz 21:d1faccb96146 256 * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU.
isagmz 21:d1faccb96146 257 *
isagmz 21:d1faccb96146 258 * @param adress - register address to write to
isagmz 21:d1faccb96146 259 * @return - data from the register specified by RA
isagmz 21:d1faccb96146 260 */
isagmz 21:d1faccb96146 261 char read( char adress);
isagmz 21:d1faccb96146 262
isagmz 21:d1faccb96146 263 /**
isagmz 21:d1faccb96146 264 * Read multtiple regigsters from the device, more efficient than using multiple normal reads.
isagmz 21:d1faccb96146 265 *
isagmz 21:d1faccb96146 266 * @param adress - register address to write to
isagmz 21:d1faccb96146 267 * @param length - number of bytes to read
isagmz 21:d1faccb96146 268 * @param data - pointer where the data needs to be written to
isagmz 21:d1faccb96146 269 */
isagmz 21:d1faccb96146 270 void read( char adress, char *data, int length);
isagmz 21:d1faccb96146 271
isagmz 21:d1faccb96146 272 /**
isagmz 21:d1faccb96146 273 * function for calculating the angle from the accelerometer.
isagmz 21:d1faccb96146 274 * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
isagmz 21:d1faccb96146 275 * for pitch, roll and one more direction.. (NOT YAW!)
isagmz 21:d1faccb96146 276 *
isagmz 21:d1faccb96146 277 * @param data - angle calculated using only accelerometer
isagmz 21:d1faccb96146 278 *
isagmz 21:d1faccb96146 279 */
isagmz 21:d1faccb96146 280 void getAcceleroAngle( float *data );
isagmz 21:d1faccb96146 281
isagmz 21:d1faccb96146 282
isagmz 21:d1faccb96146 283 /**function which allows to produce the offset values for gyro and accelerometer.
isagmz 21:d1faccb96146 284 * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
isagmz 21:d1faccb96146 285 * but offset for accelerometer is calculated in angles... later on might change that
isagmz 21:d1faccb96146 286 * function simply takes the number of samples to be taken and calculated the average
isagmz 21:d1faccb96146 287 *
isagmz 21:d1faccb96146 288 * @param accOffset - accelerometer offset in angle
isagmz 21:d1faccb96146 289 * @param gyroOffset - gyroscope offset in rad/s
isagmz 21:d1faccb96146 290 * @param sampleSize - number of samples to be taken for calculating offsets
isagmz 21:d1faccb96146 291 *
isagmz 21:d1faccb96146 292 */
isagmz 21:d1faccb96146 293 void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
isagmz 21:d1faccb96146 294
isagmz 21:d1faccb96146 295 /**
isagmz 21:d1faccb96146 296 * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
isagmz 21:d1faccb96146 297 * we also need to know how much time passed from previous calculation to now
isagmz 21:d1faccb96146 298 * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
isagmz 21:d1faccb96146 299 * if anyone need smth different, they can update this library...
isagmz 21:d1faccb96146 300 *
isagmz 21:d1faccb96146 301 * @param angle - calculated accurate angle from complemetary filter
isagmz 21:d1faccb96146 302 * @param accOffset - offset in angle for the accelerometer
isagmz 21:d1faccb96146 303 * @param gyroOffset - offset in rad/s for the gyroscope
isagmz 21:d1faccb96146 304 * @param interval - time before previous angle calculation and now
isagmz 21:d1faccb96146 305 *
isagmz 21:d1faccb96146 306 */
isagmz 21:d1faccb96146 307 void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
isagmz 21:d1faccb96146 308
isagmz 21:d1faccb96146 309 ///function, which enables interrupts on MPU6050 INT pin
isagmz 21:d1faccb96146 310 void enableInt( void );
isagmz 21:d1faccb96146 311
isagmz 21:d1faccb96146 312 ///disables interrupts
isagmz 21:d1faccb96146 313 void disableInt( void );
isagmz 21:d1faccb96146 314
isagmz 21:d1faccb96146 315 /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
isagmz 21:d1faccb96146 316 *
isagmz 21:d1faccb96146 317 * @param val - value the alpha (complementary filter constant) should be set to
isagmz 21:d1faccb96146 318 *
isagmz 21:d1faccb96146 319 */
isagmz 21:d1faccb96146 320 void setAlpha(float val);
isagmz 21:d1faccb96146 321
isagmz 21:d1faccb96146 322 private:
isagmz 21:d1faccb96146 323
isagmz 21:d1faccb96146 324 I2C connection;
isagmz 21:d1faccb96146 325 char currentAcceleroRange;
isagmz 21:d1faccb96146 326 char currentGyroRange;
isagmz 21:d1faccb96146 327 float alpha;
isagmz 21:d1faccb96146 328
isagmz 21:d1faccb96146 329 };
isagmz 21:d1faccb96146 330
isagmz 21:d1faccb96146 331
isagmz 21:d1faccb96146 332
isagmz 21:d1faccb96146 333 #endif