ESE519 Lab6 Part3
Dependencies: MPU6050_Lab6_Part3 mbed
Fork of BroBot_v2 by
BroBot_IMU.h@4:2512939c10f0, 2016-10-18 (annotated)
- Committer:
- csharer
- Date:
- Tue Oct 18 20:44:21 2016 +0000
- Revision:
- 4:2512939c10f0
- Parent:
- 3:2f76ffbc5cef
brobot version 3, this is using the old MRF24J40 Lib that used uint8_t
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:2512939c10f0 | 5 | #define ANGLE_OFFSET 107 |
csharer | 3:2f76ffbc5cef | 6 | |
csharer | 3:2f76ffbc5cef | 7 | // class default I2C address is 0x68 |
csharer | 3:2f76ffbc5cef | 8 | // specific I2C addresses may be passed as a parameter here |
csharer | 3:2f76ffbc5cef | 9 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) |
csharer | 3:2f76ffbc5cef | 10 | // AD0 high = 0x69 |
csharer | 3:2f76ffbc5cef | 11 | MPU6050 mpu; |
csharer | 3:2f76ffbc5cef | 12 | //MPU6050 mpu(0x69); // <-- use for AD0 `high |
csharer | 3:2f76ffbc5cef | 13 | // MPU control/status vars |
csharer | 3:2f76ffbc5cef | 14 | |
csharer | 3:2f76ffbc5cef | 15 | float angle, angle_old; |
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 |
csharer | 4:2512939c10f0 | 22 | float dAngle; |
csharer | 4:2512939c10f0 | 23 | float new_angle; |
csharer | 3:2f76ffbc5cef | 24 | // Orientation/motion vars |
csharer | 3:2f76ffbc5cef | 25 | Quaternion q; // [w, x, y, z] quaternion container |
csharer | 3:2f76ffbc5cef | 26 | |
csharer | 3:2f76ffbc5cef | 27 | InterruptIn checkpin(CHECKPIN); |
csharer | 3:2f76ffbc5cef | 28 | |
csharer | 3:2f76ffbc5cef | 29 | // ================================================================ |
csharer | 3:2f76ffbc5cef | 30 | // === IMU === |
csharer | 3:2f76ffbc5cef | 31 | // ================================================================ |
csharer | 3:2f76ffbc5cef | 32 | // DMP FUNCTIONS |
csharer | 3:2f76ffbc5cef | 33 | // This function defines the weight of the accel on the sensor fusion |
csharer | 3:2f76ffbc5cef | 34 | // default value is 0x80 |
csharer | 3:2f76ffbc5cef | 35 | // The official invensense name is inv_key_0_96 (??) |
csharer | 3:2f76ffbc5cef | 36 | void dmpSetSensorFusionAccelGain(uint8_t gain) |
csharer | 3:2f76ffbc5cef | 37 | { |
csharer | 3:2f76ffbc5cef | 38 | // INV_KEY_0_96 |
csharer | 3:2f76ffbc5cef | 39 | mpu.setMemoryBank(0); |
csharer | 3:2f76ffbc5cef | 40 | mpu.setMemoryStartAddress(0x60); |
csharer | 3:2f76ffbc5cef | 41 | mpu.writeMemoryByte(0); |
csharer | 3:2f76ffbc5cef | 42 | mpu.writeMemoryByte(gain); |
csharer | 3:2f76ffbc5cef | 43 | mpu.writeMemoryByte(0); |
csharer | 3:2f76ffbc5cef | 44 | mpu.writeMemoryByte(0); |
csharer | 3:2f76ffbc5cef | 45 | } |
csharer | 3:2f76ffbc5cef | 46 | |
csharer | 3:2f76ffbc5cef | 47 | // Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution) |
csharer | 3:2f76ffbc5cef | 48 | float dmpGetPhi() |
csharer | 3:2f76ffbc5cef | 49 | { |
csharer | 3:2f76ffbc5cef | 50 | mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion |
csharer | 3:2f76ffbc5cef | 51 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
csharer | 3:2f76ffbc5cef | 52 | mpu.resetFIFO(); // We always reset FIFO |
csharer | 3:2f76ffbc5cef | 53 | |
csharer | 3:2f76ffbc5cef | 54 | //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll |
csharer | 3:2f76ffbc5cef | 55 | //return Phi angle (robot orientation) from quaternion DMP output |
csharer | 3:2f76ffbc5cef | 56 | return (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); |
csharer | 3:2f76ffbc5cef | 57 | } |
csharer | 3:2f76ffbc5cef | 58 | |
csharer | 3:2f76ffbc5cef | 59 | // ================================================================ |
csharer | 3:2f76ffbc5cef | 60 | // === INTERRUPT DETECTION ROUTINE === |
csharer | 3:2f76ffbc5cef | 61 | // ================================================================ |
csharer | 3:2f76ffbc5cef | 62 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high |
csharer | 4:2512939c10f0 | 63 | Serial pc1(USBTX, USBRX); |
csharer | 3:2f76ffbc5cef | 64 | void dmpDataReady() |
csharer | 3:2f76ffbc5cef | 65 | { |
csharer | 3:2f76ffbc5cef | 66 | mpuInterrupt = true; |
csharer | 3:2f76ffbc5cef | 67 | } |