1

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

1 Answer 1

0

I have used an MPU6050 successfully in my autonomous robot project for several years now, so I have some knowledge of the subject

If your code is freezing intermittently, it may well be the infamous bug in the Arduino 'Wire' code that causes the I2C receive and/or transmit functions to block under certain conditions.

Take a look at some of my posts on 'Paynter's Palace'. You'll find there is a long history of intermittent I2C hangup problems with the stock 'Wire' library. The good news is that this problem was (sorta) fixed. You can now use the stock 'Wire' library, but you still have to manually enable the watchdog timeout value to get the transmit/receive functions to unblock if they get stuck.

See this post regarding the Wire library bug fix. You will find the following lines in setup():

Wire.setWireTimeout(3000,true); 
    wireTimeoutCount = 0;
    Wire.clearWireTimeoutFlag();

These lines set a reasonable timeout value (3000 uSec in my case), reset a local variable (wireTimeoutCount) I used for debug, and reset the new Wire library timeout flag.

Note that you'll need to ensure you are using the latest Wire library code. If the above 'Wire.setWireTimout(3000, true);' compiles, then you are using the latest library. If it doesn't, then you'll have to get the latest one.

Hope this helps,

Frank

1
  • Thank you Frank. It is working now but when i integrate the codes with NRF24L01 wireless module, the MPU6050 doent seem to work. Do you know what seems to be the problem?
    – Ivan
    Jan 13, 2021 at 6:28

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.