Carter Sharer / Mbed 2 deprecated BroBot_ESE350_Skeleton

BroBot Code for ESE350 Lab6 part 3 (Skeleton)

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_ESE350 by Carter Sharer

BroBot_IMU.h@15:d6d7623a17f8, 2017-01-26 (annotated)

Committer:
arvindnr89
Date:
Thu Jan 26 21:37:24 2017 +0000
Revision:
15:d6d7623a17f8
Parent:
11:2553f5798f84 
Introduced theta controller and added some debug statements to the imu handling

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 3:2f76ffbc5cef 1 //BroBot_IMU.h
csharer 3:2f76ffbc5cef 2 // Contains everyting needed to interface with the IMU for BroBot
csharer 4:2512939c10f0 3 //Source links: https: developer.mbed.org/users/Sissors/code/MPU6050/docs/5c63e20c50f3/classMPU6050.html
csharer 4:2512939c10f0 4 // https://developer.mbed.org/users/paulbartell/code/MPU6050-DMP/file/95449a48c5c0/MPU6050_6Axis_MotionApps20.h
csharer 3:2f76ffbc5cef 5
csharer 3:2f76ffbc5cef 6 // class default I2C address is 0x68
csharer 3:2f76ffbc5cef 7 // specific I2C addresses may be passed as a parameter here
csharer 3:2f76ffbc5cef 8 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
csharer 3:2f76ffbc5cef 9 // AD0 high = 0x69
csharer 3:2f76ffbc5cef 10 MPU6050 mpu;
csharer 3:2f76ffbc5cef 11 //MPU6050 mpu(0x69); // <-- use for AD0 `high
csharer 3:2f76ffbc5cef 12 // MPU control/status vars
arvindnr89 11:2553f5798f84 13 #include "rtos_definations.h"
csharer 3:2f76ffbc5cef 14
arvindnr89 15:d6d7623a17f8 15
csharer 3:2f76ffbc5cef 16 bool dmpReady = false; // set true if DMP init was successful
csharer 3:2f76ffbc5cef 17 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
csharer 3:2f76ffbc5cef 18 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
csharer 3:2f76ffbc5cef 19 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
csharer 3:2f76ffbc5cef 20 uint16_t fifoCount; // count of all bytes currently in FIFO
csharer 3:2f76ffbc5cef 21 uint8_t fifoBuffer[64]; // FIFO storage buffer
arvindnr89 15:d6d7623a17f8 22
csharer 3:2f76ffbc5cef 23 // Orientation/motion vars
csharer 3:2f76ffbc5cef 24 Quaternion q; // [w, x, y, z] quaternion container
csharer 3:2f76ffbc5cef 25
csharer 3:2f76ffbc5cef 26 InterruptIn checkpin(CHECKPIN);
csharer 3:2f76ffbc5cef 27
csharer 3:2f76ffbc5cef 28 // ================================================================
csharer 3:2f76ffbc5cef 29 // === IMU ===
csharer 3:2f76ffbc5cef 30 // ================================================================
csharer 3:2f76ffbc5cef 31 // DMP FUNCTIONS
csharer 3:2f76ffbc5cef 32 // This function defines the weight of the accel on the sensor fusion
csharer 3:2f76ffbc5cef 33 // default value is 0x80
csharer 3:2f76ffbc5cef 34 // The official invensense name is inv_key_0_96 (??)
csharer 3:2f76ffbc5cef 35 void dmpSetSensorFusionAccelGain(uint8_t gain)
csharer 3:2f76ffbc5cef 36 {
csharer 3:2f76ffbc5cef 37 // INV_KEY_0_96
csharer 3:2f76ffbc5cef 38 mpu.setMemoryBank(0);
csharer 3:2f76ffbc5cef 39 mpu.setMemoryStartAddress(0x60);
csharer 3:2f76ffbc5cef 40 mpu.writeMemoryByte(0);
csharer 3:2f76ffbc5cef 41 mpu.writeMemoryByte(gain);
csharer 3:2f76ffbc5cef 42 mpu.writeMemoryByte(0);
csharer 3:2f76ffbc5cef 43 mpu.writeMemoryByte(0);
csharer 3:2f76ffbc5cef 44 }
csharer 3:2f76ffbc5cef 45
csharer 3:2f76ffbc5cef 46 // Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution)
arvindnr89 15:d6d7623a17f8 47 void dmpGetReadings(float * angle, float *theta)
csharer 3:2f76ffbc5cef 48 {
csharer 3:2f76ffbc5cef 49 mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
csharer 3:2f76ffbc5cef 50 mpu.dmpGetQuaternion(&q, fifoBuffer);
csharer 3:2f76ffbc5cef 51 mpu.resetFIFO(); // We always reset FIFO
csharer 3:2f76ffbc5cef 52
csharer 3:2f76ffbc5cef 53 //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
csharer 3:2f76ffbc5cef 54 //return Phi angle (robot orientation) from quaternion DMP output
arvindnr89 15:d6d7623a17f8 55 *angle = (atan2(2 * ((q.y * q.z) + (q.w * q.x)), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);
arvindnr89 15:d6d7623a17f8 56 *theta = (atan2(2 * (q.y * q.x + q.z * q.w),1 - 2 * (q.w * q.w + q.x * q.x)) * RAD2GRAD);
arvindnr89 15:d6d7623a17f8 57 return;
csharer 3:2f76ffbc5cef 58 }
csharer 3:2f76ffbc5cef 59
csharer 3:2f76ffbc5cef 60 // ================================================================
csharer 3:2f76ffbc5cef 61 // === INTERRUPT DETECTION ROUTINE ===
csharer 3:2f76ffbc5cef 62 // ================================================================
csharer 3:2f76ffbc5cef 63 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
csharer 4:2512939c10f0 64 Serial pc1(USBTX, USBRX);
csharer 3:2f76ffbc5cef 65 void dmpDataReady()
csharer 3:2f76ffbc5cef 66 {
csharer 3:2f76ffbc5cef 67 mpuInterrupt = true;
arvindnr89 11:2553f5798f84 68 osSignalSet(imu_update_thread_ID,IMU_UPDATE_SIGNAL);
csharer 3:2f76ffbc5cef 69 }