Im using the sensor MPU6050 to control movements of my robotic arm. The codes work fine when it is a standalone program but i keep encountering 'FIFO overflow' when the codes are complied into the main program. This is the code that i am using.

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;

RF24 radio(9, 8);  // CE, CSN

const byte address[6] = "00001";

const int AccReadings = 10;
//Wrist Roll
int DataX[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int ARaverage = 0;
float correct;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
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]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel 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

struct Sensor_Data
{
  int WristRoll;
  int WristPitch;
  int ShoulderLift;
  int ElbowLift;
  int ArmRotation;
};
Sensor_Data data;

//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup()
{
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();

  //Zero-fill Arrays
  for (int i = 0; i < AccReadings; i++)
  {
   DataX[i] = 0;
  }
  for (int j = 0; j < AccReadings; j++)
  {
   DataY[j] = 0;
  }
  for (int k = 0; k < AccReadings; k++)
  {
   DataY2[k] = 0;
  }

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif

  Serial.begin(115200);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(49);
  mpu.setYGyroOffset(-18);
  mpu.setZGyroOffset(9);
  mpu.setZAccelOffset(4427);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  } 
  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(")"));
  }
}

void loop()
{
  /*smoothWR();
  movementWR();
  smoothWP();
  movementWP();
  smoothSL();
  movementSL();*/
  ElbowMovement();
  radio.write(&data, sizeof(Sensor_Data));
}

void smoothWR()
{
  WRtotal = WRtotal - DataX[WRIndex];
  DataX[WRIndex] = analogRead(A0);
  WRtotal = WRtotal + DataX[WRIndex];
  WRIndex = WRIndex + 1;

  if (WRIndex >= AccReadings)
  {
    WRIndex = 0;
  }

  WRaverage = WRtotal / AccReadings;
  //Serial.println(WRaverage);
}

void movementWR()
{            
  WRaverage = map(WRaverage, 278, 419, 0, 180);
  data.WristRoll = constrain(WRaverage, 0, 180);                                      
  //Serial.println(data.WristRoll);
}

void smoothWP()
{
  WPtotal = WPtotal - DataY[WPIndex];
  DataY[WPIndex] = analogRead(A1);
  WPtotal = WPtotal + DataY[WPIndex];
  WPIndex = WPIndex + 1;

  if (WPIndex >= AccReadings)
  {
    WPIndex = 0;
  }

  WPaverage = WPtotal / AccReadings;
  //Serial.println(WPaverage);
}

void movementWP()
{            
  WPaverage = map(WPaverage, 280, 421, 0 , 135);
  data.WristPitch = constrain(WPaverage, 0, 135);                                      
  //Serial.println(data.WristPitch);
}

void smoothSL()
{
  SLtotal = SLtotal - DataY2[SLIndex];
  DataY2[SLIndex] = analogRead(A2);
  SLtotal = SLtotal + DataY2[SLIndex];
  SLIndex = SLIndex + 1;

  if (SLIndex >= AccReadings)
  {
    SLIndex = 0;
  }

  SLaverage = SLtotal / AccReadings;
  //Serial.println(SLaverage);
}

void movementSL()
{                       
  SLaverage = map(SLaverage, 410, 270, 0 , 180);
  data.ShoulderLift = constrain(SLaverage, 35, 180);                                   
  //Serial.println(data.ShoulderLift);
}

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

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) 
  {
    if (mpuInterrupt && fifoCount < packetSize) 
    {
      // try to get out of the infinite loop
      fifoCount = mpu.getFIFOCount();
    }
  }

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

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

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) 
  {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } 
  else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) 
  {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

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

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

    // Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // Yaw, Pitch, Roll values - Radians to degrees
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process)
    if (int l = 0; l <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
      l++;
    }
    // After 300 readings
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
      ELaverage = map(ypr[0], -90, 90, 0, 180);
      data.ElbowLift = constrain(ELaverage, 30, 110);
      ARaverage = map(ypr[1], -90, 90, 0, 180);
      data.ArmRotation = constrain(ARaverage, 0, 180);     
      //Serial.println(data.ElbowLift);
      Serial.println(ypr[1]);
    }
#endif
  }
}

Is there any ways to get rid of the FIFO overflow? Also When i tried to used Jeff Rowberg's example codes MPU6050_DMP6 the program will freeze after a few seconds. Is there any solution to that? These are the example codes that i am using.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
float correct;
int j = 0;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
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]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel 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

//Interrupt Detection
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

void setup()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
  #endif
  Serial.begin(38400);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  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(")"));
  }
}

void loop()
{
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
  {
    #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            if (j <= 300) 
            {
              correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
              j++;
            }
            else
            {
              ypr[0] = ypr[0] - correct;
              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
  }
}

I'm using the sensor MPU6050 to control movements of my robotic arm. The code works fine when it is a standalone program but I keep encountering 'FIFO overflow' when the code is compiled into the main program. This is the code that I am using:

    #include <SPI.h>
    #include <nRF24L01.h>
    #include <RF24.h>
    #include "I2Cdev.h"
    #include "MPU6050_6Axis_MotionApps20.h"
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
    #endif
    MPU6050 mpu;
    
    RF24 radio(9, 8);  // CE, CSN
    
    const byte address[6] = "00001";
    
    const int AccReadings = 10;
    //Wrist Roll
    int DataX[AccReadings];
    int WRIndex = 0;
    int WRtotal = 0;
    int WRaverage = 0;
    //Wrist Pitch
    int DataY[AccReadings];
    int WPIndex = 0;
    int WPtotal = 0;
    int WPaverage = 0;
    //Shoulder Lift
    int DataY2[AccReadings];
    int SLIndex = 0;
    int SLtotal = 0;
    int SLaverage = 0;
    //Elbow Lift
    int ELaverage = 0;
    //Arm Rotation
    int ARaverage = 0;
    float correct;
    
    #define OUTPUT_READABLE_YAWPITCHROLL
    #define INTERRUPT_PIN 2
    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]            accel sensor measurements
    VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
    VectorInt16 aaWorld;    // [x, y, z]            world-frame accel 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
    
    struct Sensor_Data
    {
      int WristRoll;
      int WristPitch;
      int ShoulderLift;
      int ElbowLift;
      int ArmRotation;
    };
    Sensor_Data data;
    
    //Interrupt Detection
    volatile bool mpuInterrupt = false;
    void dmpDataReady()
    {
      mpuInterrupt = true;
    }
    
    void setup()
    {
      radio.begin();
      radio.openWritingPipe(address);
      radio.setPALevel(RF24_PA_MIN);
      radio.stopListening();
    
      //Zero-fill Arrays
      for (int i = 0; i < AccReadings; i++)
      {
       DataX[i] = 0;
      }
      for (int j = 0; j < AccReadings; j++)
      {
       DataY[j] = 0;
      }
      for (int k = 0; k < AccReadings; k++)
      {
       DataY2[k] = 0;
      }
    
      #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000);
      #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
      #endif
    
      Serial.begin(115200);
      while (!Serial);
    
      mpu.initialize();
      pinMode(INTERRUPT_PIN, INPUT);
      devStatus = mpu.dmpInitialize();
      mpu.setXGyroOffset(49);
      mpu.setYGyroOffset(-18);
      mpu.setZGyroOffset(9);
      mpu.setZAccelOffset(4427);
    
      if (devStatus == 0) 
      {
        mpu.setDMPEnabled(true);
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
      } 
      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(")"));
      }
    }
    
    void loop()
    {
      /*smoothWR();
      movementWR();
      smoothWP();
      movementWP();
      smoothSL();
      movementSL();*/
      ElbowMovement();
      radio.write(&data, sizeof(Sensor_Data));
    }
    
    void smoothWR()
    {
      WRtotal = WRtotal - DataX[WRIndex];
      DataX[WRIndex] = analogRead(A0);
      WRtotal = WRtotal + DataX[WRIndex];
      WRIndex = WRIndex + 1;
    
      if (WRIndex >= AccReadings)
      {
        WRIndex = 0;
      }
    
      WRaverage = WRtotal / AccReadings;
      //Serial.println(WRaverage);
    }
    
    void movementWR()
    {            
      WRaverage = map(WRaverage, 278, 419, 0, 180);
      data.WristRoll = constrain(WRaverage, 0, 180);                                      
      //Serial.println(data.WristRoll);
    }
    
    void smoothWP()
    {
      WPtotal = WPtotal - DataY[WPIndex];
      DataY[WPIndex] = analogRead(A1);
      WPtotal = WPtotal + DataY[WPIndex];
      WPIndex = WPIndex + 1;
    
      if (WPIndex >= AccReadings)
      {
        WPIndex = 0;
      }
    
      WPaverage = WPtotal / AccReadings;
      //Serial.println(WPaverage);
    }
    
    void movementWP()
    {            
      WPaverage = map(WPaverage, 280, 421, 0 , 135);
      data.WristPitch = constrain(WPaverage, 0, 135);                                      
      //Serial.println(data.WristPitch);
    }
    
    void smoothSL()
    {
      SLtotal = SLtotal - DataY2[SLIndex];
      DataY2[SLIndex] = analogRead(A2);
      SLtotal = SLtotal + DataY2[SLIndex];
      SLIndex = SLIndex + 1;
    
      if (SLIndex >= AccReadings)
      {
        SLIndex = 0;
      }
    
      SLaverage = SLtotal / AccReadings;
      //Serial.println(SLaverage);
    }
    
    void movementSL()
    {                       
      SLaverage = map(SLaverage, 410, 270, 0 , 180);
      data.ShoulderLift = constrain(SLaverage, 35, 180);                                   
      //Serial.println(data.ShoulderLift);
    }
    
    void ElbowMovement()
    {
      // if programming failed, don't try to do anything
      if (!dmpReady) return;
    
      // wait for MPU interrupt or extra packet(s) available
      while (!mpuInterrupt && fifoCount < packetSize) 
      {
        if (mpuInterrupt && fifoCount < packetSize) 
        {
          // try to get out of the infinite loop
          fifoCount = mpu.getFIFOCount();
        }
      }
    
      // reset interrupt flag and get INT_STATUS byte
      mpuInterrupt = false;
      mpuIntStatus = mpu.getIntStatus();
    
      // get current FIFO count
      fifoCount = mpu.getFIFOCount();
    
      // check for overflow (this should never happen unless our code is too inefficient)
      if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) 
      {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));
    
        // otherwise, check for DMP data ready interrupt (this should happen frequently)
      } 
      else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) 
      {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    
        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
    
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;
    
        // Get Yaw, Pitch and Roll values
    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    
        // Yaw, Pitch, Roll values - Radians to degrees
        ypr[0] = ypr[0] * 180 / M_PI;
        ypr[1] = ypr[1] * 180 / M_PI;
        ypr[2] = ypr[2] * 180 / M_PI;
        
        // Skip 300 readings (self-calibration process)
        if (int l = 0; l <= 300) {
          correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
          l++;
        }
        // After 300 readings
        else {
          ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
          // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
          ELaverage = map(ypr[0], -90, 90, 0, 180);
          data.ElbowLift = constrain(ELaverage, 30, 110);
          ARaverage = map(ypr[1], -90, 90, 0, 180);
          data.ArmRotation = constrain(ARaverage, 0, 180);     
          //Serial.println(data.ElbowLift);
          Serial.println(ypr[1]);
        }
    #endif
      }
    }

Is there any ways to get rid of the FIFO overflow? Also When I tried to used Jeff Rowberg's example code MPU6050_DMP6 the program froze after a few seconds. Is there any solution to that?

This is the example code that I am using:

    #include "I2Cdev.h"
    #include "MPU6050_6Axis_MotionApps20.h"
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
    #endif
    MPU6050 mpu;
    float correct;
    int j = 0;
    
    #define OUTPUT_READABLE_YAWPITCHROLL
    #define INTERRUPT_PIN 2
    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]            accel sensor measurements
    VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
    VectorInt16 aaWorld;    // [x, y, z]            world-frame accel 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
    
    //Interrupt Detection
    volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    void dmpDataReady() {
      mpuInterrupt = true;
    }
    
    void setup()
    {
      #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000);
      #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
      #endif
      Serial.begin(38400);
      while (!Serial);
    
      mpu.initialize();
      pinMode(INTERRUPT_PIN, INPUT);
      devStatus = mpu.dmpInitialize();
      mpu.setXGyroOffset(17);
      mpu.setYGyroOffset(-69);
      mpu.setZGyroOffset(27);
      mpu.setZAccelOffset(1551);
    
      if (devStatus == 0) 
      {
        mpu.setDMPEnabled(true);
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
      }
      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(")"));
      }
    }
    
    void loop()
    {
      if (!dmpReady) return;
      if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
      {
        #ifdef OUTPUT_READABLE_YAWPITCHROLL
                // display Euler angles in degrees
                mpu.dmpGetQuaternion(&q, fifoBuffer);
                mpu.dmpGetGravity(&gravity, &q);
                mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
                if (j <= 300) 
                {
                  correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
                  j++;
                }
                else
                {
                  ypr[0] = ypr[0] - correct;
                  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
      }
    }