MPU6050

#include "I2Cdev.h"                                           // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
#include "MPU6050_6Axis_MotionApps20.h"
#include "MPU6050.h"                                          // not necessary if using MotionApps include file

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;
//MPU6050 mpu(0x69);                                          // <-- use for AD0 high

/* ======================
   | Pin   | Connection |
   ======================
   | A4    | (SDA)      |
   | A5    | (SCL)      |
   | 3.3V  | VCC        |
   | GND   | GND        |
   | 2     | INIT       |
   ====================== */

// Uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)

// #define OUTPUT_READABLE_QUATERNION

// Uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock - http://en.wikipedia.org/wiki/Gimbal_lock

// #define OUTPUT_READABLE_EULER

// Uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock - http://en.wikipedia.org/wiki/Gimbal_lock

#define OUTPUT_READABLE_YAWPITCHROLL

// Uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.

// #define OUTPUT_READABLE_REALACCEL

// Uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.

// #define OUTPUT_READABLE_WORLDACCEL

// Uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo

// #define OUTPUT_TEAPOT

#define INTERRUPT_PIN 2           // INIT for MPU6050
#define LED_PIN 13                // Arduino is 13 for Indication

bool blinkState = false;

// MPU Control / Status Vars
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

// Orientation / Motion Vars
Quaternion q;                     // [w, x, y, z]         Quaternion Container
VectorInt16 aa;                   // [x, y, z]            Accelorometer Sensor Measurements
VectorInt16 aaReal;               // [x, y, z]            Gravity - Free Accelorometer Sensor Measurements
VectorInt16 aaWorld;              // [x, y, z]            World - Frame Accelorometer Sensor Measurements
VectorFloat gravity;              // [x, y, z]            Gravity Vector
float euler[3];                   // [psi, theta, phi]    Euler angle container
float ypr[3];                     // [yaw, pitch, roll]   Yaw / Pitch / Roll Container and Gravity Vector

// Packet Structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady()
{
    mpuInterrupt = true;
}

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup()
{
    // Join I2C bus (I2Cdev Library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000);
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // Initialize Serial Communication (115200 chosen because it is required for Teapot Demo output, but it's really up to you depending on your project)
    Serial.begin(115200);

    while (!Serial);
    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // Initialize Device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

    // Verify Connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // Wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read());                  // Empty Buffer
    while (!Serial.available());                                  // Wait for Data
    while (Serial.available() && Serial.read());                  // Empty Buffer Again

    // Load and Configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // Supply your own Gyro Offsets here, scaled for Minimum Sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);                                    // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        Serial.println(F(" Enabling DMP... "));                   // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        Serial.println(F(" Enabling interrupt detection (Arduino external interrupt 0)... "));      // Enable Arduino interrupt detection
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        Serial.println(F(" DMP ready! Waiting for first interrupt... "));                           // Set our DMP Ready flag so the main loop() function knows it's okay to use it
        dmpReady = true;

        packetSize = mpu.dmpGetFIFOPacketSize();                                                    // get expected DMP packet size for later comparison
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F(" DMP Initialization failed (code : "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    pinMode(LED_PIN, OUTPUT);                                                                     // configure LED for output
}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop()
{
    if (!dmpReady)                                                                                // if programming failed, don't try to do anything
      return;

    while (!mpuInterrupt && fifoCount < packetSize)                                               // wait for MPU interrupt or extra packet(s) available
    {
        // other program behavior stuff here
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
    }

    mpuInterrupt = false;                                                                         // reset interrupt flag and get INT_STATUS byte
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();                                                               // get current FIFO count

    if ((mpuIntStatus & 0x10) || fifoCount == 1024)                                               // Check for overflow (this should never happen unless our code is too inefficient)
    {
        mpu.resetFIFO();                                                                          // reset so we can continue cleanly
        Serial.println(F("FIFO overflow!"));
    }
    else if (mpuIntStatus & 0x02)                                                                 // Otherwise, check for DMP data ready interrupt (this should happen frequently)
    {
        while (fifoCount < packetSize)                                                            // Wait for correct available data length, should be a VERY short wait
          fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, packetSize);                                                 // Read a packet from FIFO

        fifoCount -= packetSize;                                                                  // Track FIFO count here in case there is > 1 packet available (this lets us immediately read more without waiting for an interrupt)

        #ifdef OUTPUT_READABLE_QUATERNION                                                         // Display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("Quat : \t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER                                                              // Display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("Euler : \t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL                                                       // Display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("YPR : \t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL                                                          // Display Real Acceleration, Adjusted to Remove Gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL                                                         // Display Initial World-Frame Acceleration, Adjusted to Remove Gravity and Rotated based on known Orientation from Quaternion
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);
        #endif

        #ifdef OUTPUT_TEAPOT                                                                        // Display Quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++;                                                                     // PacketCount, Loops at 0xFF on purpose
        #endif

        blinkState = !blinkState;                                                                   // Blink LED to Indicate Activity
        digitalWrite(LED_PIN, blinkState);
    }
}