Проблемы переполнения и зависания MPU6050 FIFO

Я использую датчик MPU6050 для управления движениями моей роботизированной руки. Код отлично работает, когда это автономная программа, но я продолжаю сталкиваться с "переполнением FIFO", когда код компилируется в основную программу. Это код, который я использую:

    #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
      }
    }

Есть ли какие-либо способы избавиться от переполнения FIFO? Также, когда я попытался использовать пример кода Джеффа Роуберга MPU6050_DMP6, программа замерла через несколько секунд. Есть ли какое-нибудь решение для этого?

Это пример кода, который я использую:

    #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
      }
    }