MPU6050 DMP gets stuck with Arduino Due

Hi Community!

I am a newbie in coding and have taken on a relatively big task - to create the Cubli- Self-balancing cube. It is a similar concept to a Self-balancing robot, the only difference is here, the cube balances on its edge/corner using reaction wheels.

The issue we are facing is with respect to the IMU (MPU6050). For the project, we have used an Arduino Due and made changes to the actual MPU6050.cpp file (attached here) for the microcontroller.

MPU6050.cpp (133.0 KB)

The IMU works fine for most parts of the code but once it enters the My_Handler_LQR(), the code seems to stop working after a few seconds - the DMP gets stuck and starts giving a constant value repeatedly.

What we have tried-
a. Check for FIFO overflow - no it does not overflow
b. All connections are proper
c. Wires are connected properly
d. The sampling rate was reduced to 10kHz for the MPU6050 and still gave the same error
e. We tried getting readings while connecting only the MPU6050 and it still gave an error
f. The DMP example code works perfectly (by Jeff Rowberg)

//----------------------------------------------- ALL LIBRARY INCLUSIONS ------------------------------------- //
#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
#define INTERRUPT_PIN 2  //mpu

//----------------------------------------------- ALL DECLARATIONS -------------------------------------------- //

Servo ESC;

//-------IMU------//
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

Quaternion q;           // [w, x, y, z]         quaternion container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
VectorFloat gravity;    // [x, y, z]            gravity 
int16_t gyro[3];

MPU6050 mpu(0x69); // <-- use for AD0 high
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

//--------SPEED ENCODER-------//
int encoder_pin = 5;
unsigned int rpm;
volatile int pulses = 0; 
unsigned long timeold = 0;
unsigned long timeold_1 =0;
unsigned int pulsesperturn = 10;

//-------LQR------//
#define K1 -0.9
#define K2 -1.2
#define K3 -0

float mpu_pos = 0;
float mpu_vel = 0;
float control = 0 ;

int ctr=0;

//-------PID------//
double kp = 0.00006;
double ki = 0.00014;
double kd = 0.00004;
 
unsigned long currentTime, previousTime;
double elapsedTime;
double error;
double lastError;
double input, output;
double Setpoint = 0.00;
double cumError, rateError;
double control_PID = 0;



//----------------------------------------------- ALL FUNCTIONS -------------------------------------------- //

//--------------------IMU AND DMP------------------//
void dmpDataReady() 
{
    mpuInterrupt = true;
}

void DMP_initialize()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
      #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
      #endif
      while (!Serial);
      Serial.println(F("Initializing I2C devices..."));
      mpu.initialize();
      pinMode(INTERRUPT_PIN, INPUT);
      Serial.println(F("Testing device connections..."));
      Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
      Serial.println(F("Initializing DMP..."));
      devStatus = mpu.dmpInitialize();
    
      mpu.setXGyroOffset(92);
      mpu.setYGyroOffset(-32);
      mpu.setZGyroOffset(-41);
      mpu.setZAccelOffset(751); // 1688 factory default for my test chip

      if (devStatus == 0) {
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        mpu.PrintActiveOffsets();
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        Serial.println(F("DMP ready!"));
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
     } else {
        
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
     }
}
   
float return_vel()
{ 
  mpu.dmpGetGyro(gyro, fifoBuffer);
  return(gyro[2]);
}

float return_roll()
{
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  return(ypr[2]*180/M_PI);
}


//---------------CONTROL MECHANISMS------------//
double computePID(double inp)
{     
        currentTime = millis();                //get current time
        elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation
        
        error = Setpoint - inp;                                // determine error
        
        cumError += error * elapsedTime;                // compute integral
        rateError = (error - lastError)/elapsedTime;   // compute derivative
 
        double out = kp*error + ki*cumError + kd*rateError;                //PID output               
        Serial.println(out);
        lastError = error;                                //remember current error
        previousTime = currentTime;                        //remember current time
 
        return out;                                        //have function return the PID output
}

void myHandler_PID(float mpu_pos)
{
  control_PID = computePID(double(mpu_pos));
  if (control_PID>12)
  {
    control_PID=12;
  }
  else if (control_PID<-12)
  {
    control_PID=-12;
  }
  int control_out_PID = map(control_PID, -12, 12, -180, 180);
  Serial.println(control_out_PID);
  if(control_out_PID>0) 
  {digitalWrite(8, HIGH);}
  else
  {digitalWrite(8,LOW);}
  ESC.write(abs(control_out_PID));
}

void myHandler_LQR(float mpu_pos_1,float mpu_vel_1,int wheel_spd_1)
{
  control = K1*mpu_pos_1 + K2*mpu_vel_1 + K3*wheel_spd_1;
  //Serial.println(control);
  if (control>12)
  {
    control=12;
  }
  else if (control<-12)
  {
    control=-12;
  }
  
  Serial.println(control);
  //int control_out = map(control, -12, 12, -180, 180);
  //Serial.println(control_out);
  control = (control/12)*180;
  if(control>0) 
  {digitalWrite(8, HIGH);}
  else
  {digitalWrite(8,LOW);}
  ESC.write(abs(control));
}


void counter()
{
   pulses++;
}

int return_speed()
{
      detachInterrupt(encoder_pin);
      rpm = (60*1000 / pulsesperturn )/ (millis() - timeold)* pulses;
      timeold = millis();
      pulses = 0;
      attachInterrupt(encoder_pin, counter, FALLING);
      return((int)rpm);
}
      
void speed_up()
{
  int sensorValue = analogRead(A2);
  int pwm = map(sensorValue, 0, 1023, 0, 180);
  Serial.println(pwm);
  ESC.write(pwm);  
}

void direction_control(int ctr)
{
    if(ctr==0)
    {
      digitalWrite(8,HIGH);
    }
    else
    {
      digitalWrite(8,LOW);
    }
  }

void main_loop_LQR()
{
    if (!dmpReady) return;
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu_pos = return_roll();
    mpu_vel = return_vel();
    }   
    int spd = return_speed();
    if(mpu_pos>3 || mpu_pos<-3){
    myHandler_LQR(mpu_pos,mpu_vel,spd);}    
// Comment the following print statements, based on what value we're debugging
    Serial.print("POS = ");
    Serial.print(mpu_pos);
//    Serial.print("  ");
//    Serial.print("VEL = ");
//    Serial.print(mpu_vel);
//    Serial.print("  ");
//    Serial.print("WHEEL SPEED = ");
//    Serial.print(spd);
    Serial.println();
    timeold_1=millis();
}

void main_loop_PID()
{
  if (!dmpReady) return;
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu_pos = return_roll();
    }
    if(mpu_pos>3 || mpu_pos<-3){
    myHandler_PID(mpu_pos);}
    timeold_1=millis();
}

//----------------------------------------------- SETUP ----------------------------------------------- //
void setup() 
{
  Serial.begin(9600);
  DMP_initialize();
  ESC.attach(9,0,2000);   //speed control for BLDC motor
  pinMode(3,INPUT_PULLUP);   //cotrol input for switching directions manually
  pinMode(8,OUTPUT);      //control output for switching directions
  pinMode(encoder_pin, INPUT);
  attachInterrupt(encoder_pin, counter, FALLING);
  timeold = millis();
  timeold_1 = millis();
}

//----------------------------------------------- MAIN LOOP -------------------------------------------- //

void loop() 
{
    if (millis() - timeold_1 >= 1)
    {
      main_loop_LQR();         //Use for LQR Control
      //main_loop_PID();       //Use for PID Control
    }
    //speed_up();   //Use these two lines for manual control
    //direction_control(digitalRead(3));
//The following lines are present to be used only while checking code that ONLY gets IMU values
    //if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    //mpu_pos = return_roll();
   // Serial.println(return_roll());}
}

//----------------------------------------------- CODE END ---------------------------------------------- //

Note: although we have written the code for the PID, we aren’t using it at the moment

Hi, I’ve had problems with both the Due and MEGA2560 when using both I2C and Serial together.
I notice the Serial Baud rate you are using is only 9600. and there is little limit on how fast you are calling main_loop_LQR. You could try upping this dramatically and re-run and/or remove the output to Serial altogether.

Hi!
We actuallt did try removing the Serial print statements all together and connected the Due to an external power source
It still gives the same response