Diff: balance_bot_IMU.h

Revision:

6:ae3e6aefe908

Parent:

4:2512939c10f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/balance_bot_IMU.h	Thu Nov 10 19:20:55 2016 +0000
@@ -0,0 +1,66 @@
+//balance_bot_IMU.h
+// Contains everyting needed to interface with the IMU for balancebot 
+//Source links: https: developer.mbed.org/users/Sissors/code/MPU6050/docs/5c63e20c50f3/classMPU6050.html
+// https://developer.mbed.org/users/paulbartell/code/MPU6050-DMP/file/95449a48c5c0/MPU6050_6Axis_MotionApps20.h
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 mpu;
+//MPU6050 mpu(0x69); // <-- use for AD0 `high
+// MPU control/status vars
+
+float angle, angle_old;
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+float dAngle;
+float new_angle;
+// Orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+
+InterruptIn checkpin(CHECKPIN);
+
+// ================================================================
+// ===                       IMU                                ===
+// ================================================================
+// DMP FUNCTIONS
+// This function defines the weight of the accel on the sensor fusion
+// default value is 0x80
+// The official invensense name is inv_key_0_96 (??)
+void dmpSetSensorFusionAccelGain(uint8_t gain)
+{
+    // INV_KEY_0_96
+    mpu.setMemoryBank(0);
+    mpu.setMemoryStartAddress(0x60);
+    mpu.writeMemoryByte(0);
+    mpu.writeMemoryByte(gain);
+    mpu.writeMemoryByte(0);
+    mpu.writeMemoryByte(0);
+}
+
+// Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution)
+float dmpGetPhi()
+{
+    mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
+    mpu.dmpGetQuaternion(&q, fifoBuffer);
+    mpu.resetFIFO();  // We always reset FIFO
+
+    //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
+    //return Phi angle (robot orientation) from quaternion DMP output
+    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);
+}
+
+// ================================================================
+// ===               INTERRUPT DETECTION ROUTINE                ===
+// ================================================================
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+Serial pc1(USBTX, USBRX);
+void dmpDataReady()
+{
+    mpuInterrupt = true;
+}
\ No newline at end of file