JJRobots COMMUNITY

Full Version: MPU6050 FIFO gets corrupted after a while
You're currently viewing a stripped down version of our content. View the full version with proper formatting.
Using JJRobot's BROBOT.ino as reference, I have written different ISR to suit my L298N drivers and the balancing works well.

However, after a minute I start running the code, the MPU stops responding. I tried to debug where the problem might be. It seems the mpu.getFIFOCount() always returns 0 after a while. I am using JJ_MPU6050_DMP_6Axis.h header file without any changes.

I thought MPU was conflicting with the timer and tried disabling it. But MPU still stops responding after a while. I'm using Arduino Uno as microcontroller. Is this a limitation of this setup and should I look into processing raw mpu data? How can I get FIFO to work?

My Arduino code is pasted here
Code:
#define TARGET_ANGLE 1.7

#include <Wire.h>
#include <I2Cdev.h>                // I2Cdev lib from www.i2cdevlib.com
#include <JJ_MPU6050_DMP_6Axis.h>  // Modified version of the MPU6050 library to work with DMP (see comments inside)
// This version optimize the FIFO (only contains quaternion) and minimize code size

// NORMAL MODE PARAMETERS (MAXIMUN SETTINGS)
#define MAX_THROTTLE 580
#define MAX_STEERING 150
#define MAX_TARGET_ANGLE 12

// PRO MODE = MORE AGGRESSIVE (MAXIMUN SETTINGS)
#define MAX_THROTTLE_PRO 980 //680
#define MAX_STEERING_PRO 250
#define MAX_TARGET_ANGLE_PRO 40 //20

// Default control terms
#define KP 0.19        
#define KD 28          
#define KP_THROTTLE 0.07    
#define KI_THROTTLE 0.04  

// Control gains for raiseup (the raiseup movement requiere special control parameters)
#define KP_RAISEUP 0.16
#define KD_RAISEUP 36
#define KP_THROTTLE_RAISEUP 0   // No speed control on raiseup
#define KI_THROTTLE_RAISEUP 0.0

#define MAX_CONTROL_OUTPUT 500

// Servo definitions
#define SERVO_AUX_NEUTRO 1550  // Servo neutral position
#define SERVO_MIN_PULSEWIDTH 650
#define SERVO_MAX_PULSEWIDTH 2600

// Battery management [optional]. This is not needed for alkaline or Ni-Mh batteries but usefull for if you use lipo batteries
#define BATTERY_CHECK 1                // 0: No  check, 1: check (send message to interface)
#define LIPOBATT 0             // Default 0: No Lipo batt 1: Lipo batt (to adjust battery monitor range)
#define BATTERY_WARNING 105    // (10.5 volts) aprox [for lipo batts, disable by default]
#define BATTERY_SHUTDOWN 95   // (9.5 volts) [for lipo batts]
#define SHUTDOWN_WHEN_BATTERY_OFF 0    // 0: Not used, 1: Robot will shutdown when battery is off (BATTERY_CHECK SHOULD BE 1)

#define DEBUG 0   // 0 = No debug info (default)

#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))

#define ZERO_SPEED 65535
#define MAX_ACCEL 7        // Maximun motor acceleration (MAX RECOMMENDED VALUE: 8) (default:7)

#define MICROSTEPPING 16   // 8 or 16 for 1/8 or 1/16 driver microstepping (default:16)

#define I2C_SPEED 400000L  // 400kHz I2C speed

#define RAD2GRAD 57.2957795
#define GRAD2RAD 0.01745329251994329576923690768489

#define ITERM_MAX_ERROR 25   // Iterm windup constants for PI control //40
#define ITERM_MAX 8000       // 5000

bool Robot_shutdown = false; // Robot shutdown flag => Out of

String MAC;  // MAC address of Wifi module

// 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 (for us 18 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[18]; // FIFO storage buffer
Quaternion q;

uint8_t loop_counter;       // To generate a medium loop 40Hz
uint8_t slow_loop_counter;  // slow loop 2Hz
uint8_t sendBattery_counter; // To send battery status

long timer_old;
long timer_value;
int debug_counter;
float debugVariable;
float dt;

// class default I2C address is 0x68 for MPU6050
MPU6050 mpu;

// Angle of the robot (used for stability control)
float angle_adjusted;
float angle_adjusted_Old;

// Default control values from constant definitions
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE;
float Ki_thr = KI_THROTTLE;
float Kp_user = KP;
float Kd_user = KD;
float Kp_thr_user = KP_THROTTLE;
float Ki_thr_user = KI_THROTTLE;
bool newControlParameters = false;
bool modifing_control_parameters = false;
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
float throttle;
float steering;
float max_throttle = MAX_THROTTLE;
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;

uint8_t mode;  // mode = 0 Normal mode, mode = 1 Pro mode (More agressive)

int16_t motor1;
int16_t motor2;

int16_t speed_M1, speed_M2;        // Actual speed of motors
int8_t  dir_M1, dir_M2;            // Actual direction of steppers motors
int16_t actual_robot_speed;        // overall robot speed (measured from steppers speed)
int16_t actual_robot_speed_Old;
float estimated_speed_filtered;    // Estimated robot speed

// Stepper motor
const int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution
int motor1_pin_1 = 8;
int motor1_pin_2 = 9;
int motor1_pin_3 = 10;
int motor1_pin_4 = 11;
int motor1_step_number = 0;
int motor1_direction = 0;

int motor2_pin_1 = 4;
int motor2_pin_2 = 5;
int motor2_pin_3 = 6;
int motor2_pin_4 = 7;
int motor2_step_number = 0;
int motor2_direction = 0;

// DMP FUNCTIONS
// This function defines the weight of the accel on the sensor fusion
// default value is 0x80
// The official invensense name is inv_key_0_96 (??)
void dmpSetSensorFusionAccelGain(uint8_t gain)
{
  // INV_KEY_0_96
  mpu.setMemoryBank(0);
  mpu.setMemoryStartAddress(0x60);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(gain);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(0);
}

// Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution)
float dmpGetPhi() {
  mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.resetFIFO();  // We always reset FIFO

  return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
  //return Phi angle (robot orientation) from quaternion DMP output
  //return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);
}

// PD controller implementation(Proportional, derivative). DT is in miliseconds
float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
{
  float error;
  float output;

  error = setPoint - input;

  // Kd is implemented in two parts
  //    The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
  //    And the second using the setpoint to make it a bit more agressive   setPoint-setPoint(t-1)
  output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
  //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
  PID_errorOld2 = PID_errorOld;
  PID_errorOld = input;  // error for Kd is only the input component
  setPointOld = setPoint;
  return (output);
}


// PI controller implementation (Proportional, integral). DT is in miliseconds
float speedPIControl(float DT, float input, float setPoint,  float Kp, float Ki)
{
  float error;
  float output;

  error = setPoint - input;
  PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
  PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);

  //Serial.println(PID_errorSum);

  output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
  return (output);
}


ISR(TIMER1_COMPA_vect) {
  // TIMER 1 : STEPPER MOTOR1
  if(dir_M1 != 0) {
    if(motor1_step_number == 0) {
      digitalWrite(motor1_pin_1, HIGH);
      digitalWrite(motor1_pin_2, LOW);
      digitalWrite(motor1_pin_3, HIGH);
      digitalWrite(motor1_pin_4, LOW);
      digitalWrite(motor2_pin_1, HIGH);
      digitalWrite(motor2_pin_2, LOW);
      digitalWrite(motor2_pin_3, HIGH);
      digitalWrite(motor2_pin_4, LOW);
      if(dir_M1 == 1) motor1_step_number = 3;
      else motor1_step_number++;
    }
    else if (motor1_step_number == 1) {
      digitalWrite(motor1_pin_1, LOW);
      digitalWrite(motor1_pin_2, HIGH);
      digitalWrite(motor1_pin_3, HIGH);
      digitalWrite(motor1_pin_4, LOW);
      digitalWrite(motor2_pin_1, LOW);
      digitalWrite(motor2_pin_2, HIGH);
      digitalWrite(motor2_pin_3, HIGH);
      digitalWrite(motor2_pin_4, LOW);
      if(dir_M1 == 1) motor1_step_number--;
      else motor1_step_number++;
    }
    else if (motor1_step_number == 2) {
      digitalWrite(motor1_pin_1, LOW);
      digitalWrite(motor1_pin_2, HIGH);
      digitalWrite(motor1_pin_3, LOW);
      digitalWrite(motor1_pin_4, HIGH);
      digitalWrite(motor2_pin_1, LOW);
      digitalWrite(motor2_pin_2, HIGH);
      digitalWrite(motor2_pin_3, LOW);
      digitalWrite(motor2_pin_4, HIGH);
      if(dir_M1 == 1) motor1_step_number--;
      else motor1_step_number++;
    }
    else if (motor1_step_number == 3) {
      digitalWrite(motor1_pin_1, HIGH);
      digitalWrite(motor1_pin_2, LOW);
      digitalWrite(motor1_pin_3, LOW);
      digitalWrite(motor1_pin_4, HIGH);
      digitalWrite(motor2_pin_1, HIGH);
      digitalWrite(motor2_pin_2, LOW);
      digitalWrite(motor2_pin_3, LOW);
      digitalWrite(motor2_pin_4, HIGH);
      if(dir_M1 == 1) motor1_step_number--;
      else motor1_step_number = 0;
    }
  }
}


// Set speed of Stepper Motor1
// tspeed could be positive or negative (reverse)
void setMotorSpeedM1(int16_t tspeed)
{
  long timer_period;
  int16_t speed;

  // Limit max speed?

  // WE LIMIT MAX ACCELERATION of the motors
  if ((speed_M1 - tspeed) > MAX_ACCEL)
    speed_M1 -= MAX_ACCEL;
  else if ((speed_M1 - tspeed) < -MAX_ACCEL)
    speed_M1 += MAX_ACCEL;
  else
    speed_M1 = tspeed;

  speed = speed_M1; // No micro stepping for L298N

  if (speed == 0)
  {
    timer_period = ZERO_SPEED;
    dir_M1 = 0;
    dir_M2 = 0;
  }
  else if (speed > 0)
  {
    timer_period = 2000000 / speed; // 2Mhz timer
    dir_M1 = 1;
    dir_M2 = 1;
//    SET(PORTB, 4); // DIR Motor 1 (Forward)
  }
  else
  {
    timer_period = 2000000 / -speed;
    dir_M1 = -1;
    dir_M2 = -1;
//    CLR(PORTB, 4); // Dir Motor 1
  }
  if (timer_period > 65535)   // Check for minimun speed (maximun period without overflow)
    timer_period = ZERO_SPEED;

  OCR1A = timer_period;
  // Check  if we need to reset the timer...
  if (TCNT1 > OCR1A)
    TCNT1 = 0;
}


// INITIALIZATION
void setup()
{
  // STEPPER PINS ON JJROBOTS BROBOT BRAIN BOARD
  pinMode(motor1_pin_1, OUTPUT);
  pinMode(motor1_pin_2, OUTPUT);
  pinMode(motor1_pin_3, OUTPUT);
  pinMode(motor1_pin_4, OUTPUT);
  pinMode(motor2_pin_1, OUTPUT);
  pinMode(motor2_pin_2, OUTPUT);
  pinMode(motor2_pin_3, OUTPUT);
  pinMode(motor2_pin_4, OUTPUT);

  Serial.begin(115200); // Serial output to console

  // Wifi module initialization: Wifi module should be preconfigured (use another sketch to configure the module)

  // Initialize I2C bus (MPU6050 is connected via I2C)
  Wire.begin();
  // I2C 400Khz fast mode
  TWSR = 0;
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
  TWCR = 1 << TWEN;

#if DEBUG > 0
  delay(9000);
#else
  delay(2000);
#endif
  Serial.println("BROBOT by JJROBOTS v2.2");
  Serial.println("Initializing I2C devices...");
  //mpu.initialize();
  // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
  mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setDLPFMode(MPU6050_DLPF_BW_10);  //10,20,42,98,188  // Default factor for BROBOT:10
  mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
  mpu.setSleepEnabled(false);

  delay(500);
  Serial.println("Initializing DMP...");
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println("Enabling DMP...");
    mpu.setDMPEnabled(true);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
  }
  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("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }

  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();

  // Gyro calibration
  // The robot must be steady during initialization
  delay(500);
  Serial.println("Gyro calibration!!  Dont move the robot in 10 seconds... ");
  delay(10000);

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

  // STEPPER MOTORS INITIALIZATION
  Serial.println("Steper motors initialization...");
  // MOTOR1 => TIMER1
  TCCR1A = 0;                       // Timer1 CTC mode 4, OCxA,B outputs disconnected
  TCCR1B = (1 << WGM12) | (1 << CS11); // Prescaler=8, => 2Mhz
  OCR1A = ZERO_SPEED;               // Motor stopped
  dir_M1 = 0;
  dir_M2 = 0;
  TCNT1 = 0;

  //Adjust sensor fusion gain
  Serial.println("Adjusting DMP sensor fusion gain...");
  dmpSetSensorFusionAccelGain(0x20);

  delay(1000);

  // Enable TIMERs interrupts
  TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt

  Serial.println("Let´s start...");
  delay(100);

  mpu.resetFIFO();
  timer_old = millis();
  Robot_shutdown = false;
  mode = 0;
}


// MAIN LOOP
void loop()
{
  steering = 0;
  throttle = 0;
  timer_value = millis();

  // reset interrupt flag and get INT_STATUS byte
  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 & 0x10) || fifoCount > 18) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
//    Serial.print(F("FIFO overflow!"));
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) {
      fifoCount = mpu.getFIFOCount();
    }
  
//    loop_counter++;
    dt = (timer_value - timer_old);
    timer_old = timer_value;

    angle_adjusted_Old = angle_adjusted;
    // Get new orientation angle from IMU (MPU6050)
    angle_adjusted = dmpGetPhi();
  
    mpu.resetFIFO();  // We always reset FIFO

    // We calculate the estimated robot speed:
    // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
    actual_robot_speed_Old = actual_robot_speed;
    actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward

    int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
    int16_t estimated_speed = -actual_robot_speed_Old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
    estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;  // low pass filter on estimated speed

    // SPEED CONTROL: This is a PI controller.
    //    input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
    target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
    target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
    target_angle = TARGET_ANGLE;

    // Stability control: This is a PD controller.
    //    input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
    //    We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
    control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
    control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control

    // The steering part from the user is injected directly on the output
    motor1 = control_output + steering;
//    motor2 = control_output - steering;

    // Limit max speed (control output)
    motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);

    // NOW we send the commands to the motors
    if ((angle_adjusted < 76) && (angle_adjusted > -76)) // Is robot ready (upright?)
    {
      // NORMAL MODE
      setMotorSpeedM1(motor1);
      Kp = Kp_user;            // Default user control gains
      Kd = Kd_user;
      Kp_thr = Kp_thr_user;
      Ki_thr = Ki_thr_user;
    }
    else   // Robot not ready (flat), angle > 70º => ROBOT OFF
    {
      setMotorSpeedM1(0);
      PID_errorSum = 0;  // Reset PID I term
    }
  }
}
Try to "shield" the MPU´s i2c cable from interferences. I bought cheap stepper motors and the electromagnetic noise coming from them was getting inside the MPU´s data hanging my Brobot.
Same problem here. Solved by connecting external wires directly from the leonardo to the MPU.
Reference URL's