Stepper motor slow when reading from MPU 6050 sensor

Question

I'm trying to get an inverted pendulum project working (following the instructions here), and while my stepper motor moves the cart just fine when that's the only code in the Arduino sketch, as soon as I add the MPU 6050 sensor for detecting the angle of the pendulum, the cart moves much slower, far too slow to be useful for keeping the pendulum upright.

I imagine the timing of the motor commands is messed up by the sensor reads, but I'm not sure how to avoid the problem. Also, the author of the example posted essentially the same code as his working solution for proportional control, so there may be something else that I'm missing that messes my motor performance up.

Does anyone have any suggestions for how to get the stepper motor turning fast enough while also reading data from the MPU 6050?

My code is below. I'm using a Nema 17 stepper motor with an A4988 driver, a 12 V 10 A power supply, and this Aukru MPU-6050. To be sure that updates to the speed and Serial prints aren't the problem, I use an interval of 500 ms between updates/printing.

#include "I2Cdev.h"
#include <AccelStepper.h>
#include "MPU6050_6Axis_MotionApps20.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards

// 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 ROUTINE                ===
// ================================================================

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

// Defines pins numbers
const int stepPin = 3;
const int dirPin = 4;
AccelStepper stepper(1,stepPin,dirPin);

int motorSpeed, currentPos;
unsigned long t_start, t_elapsed;
int interval = 500; // ms between printing speed

int speedMax = 4000;
float k_proportional = 4;

float angleCurrent, speedSet;
 
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #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
    
  Serial.begin(115200);
  
  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      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(")"));
  }  
  
  stepper.setMaxSpeed(speedMax);
  stepper.setAcceleration(10000);
  stepper.setSpeed(2000);
}

void loop() {
    // 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 & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(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();

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

      // display Euler angles in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
    
  t_elapsed = millis() - t_start;
  
  if (t_elapsed >= interval) {
    t_start += interval;
    motorSpeed=setMotorSpeed();
    stepper.setSpeed(motorSpeed);
    Serial.print("Roll ");
    Serial.print(ypr[2] * 180/M_PI);
    Serial.print("Motor speed: ");
    Serial.println(motorSpeed);
  }  
 
  stepper.runSpeed();
}

int setMotorSpeed() {
  // Proportional control
  angleCurrent = ypr[2];
  speedSet = constrain(-angleCurrent*k_proportional*speedMax, -speedMax, speedMax);
  return speedSet;
}

Edit: Adding my code which limits the sensor logic to the same interval as the speed update, based on the answer by chrisl. The cart moves quite a bit faster, though still not fast enough to keep the pendulum upright.

#include "I2Cdev.h"
#include <AccelStepper.h>
#include "MPU6050_6Axis_MotionApps20.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards

// 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 ROUTINE                ===
// ================================================================

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

// Defines pins numbers
const int stepPin = 3;
const int dirPin = 4;
AccelStepper stepper(1,stepPin,dirPin);

int motorSpeed, currentPos;
unsigned long t_start, t_elapsed, t_mpu, t_start_mpu;
int interval = 50; // ms between printing speed
unsigned long count = 0;

int speedMax = 800;
float k_proportional = 3;

float angleCurrent, speedSet;
 
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #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
    
  Serial.begin(115200);
  
  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      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(")"));
  }  
  
  stepper.setMaxSpeed(speedMax);
  stepper.setAcceleration(10000);
  stepper.setSpeed(100);
}

void loop() {
    
  t_elapsed = millis() - t_start;
  count += 1;
  
  if (t_elapsed >= interval) {
    t_start += interval;
  
    // wait for MPU interrupt or extra packet(s) available
    // Slows down cart tremendously, but code freezes without it    
    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 & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(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();
      
      // 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;

      // display Euler angles in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);      
    }  
  
    motorSpeed=setMotorSpeed();
    // Use one potentiometer to turn system on or off (A0)
    int A0val = analogRead(A0);
    // Use another potentiometer to tune response time (or max speed)
    int A1val = analogRead(A1);
    //speedMax = 2*A1val;
    interval = A1val/2;
    Serial.print("Roll: ");
    Serial.print(ypr[2] * 180/M_PI);
    Serial.print(", motor speed: ");
    Serial.print(motorSpeed);
    Serial.print(", A0: ");
    Serial.print(A0val);
    Serial.print(", interval: ");
    Serial.print(interval);
    //Serial.print(", speedMax: ");
    //Serial.print(speedMax);
    Serial.print(", iterations: ");
    Serial.println(count);
    count=0;
    if (A0val > 512)
      stepper.setSpeed(motorSpeed);
    else
      stepper.setSpeed(0);
  }
 
  stepper.runSpeed();
}

int setMotorSpeed() {
  // Proportional control
  angleCurrent = ypr[2];
  speedSet = constrain(-angleCurrent*k_proportional*speedMax, -speedMax, speedMax);
  return speedSet;
}

Answer 1

I cannot say, why that principle works for the author of that tutorial. Though I can guess the reason, why your motors runs so slow.

The function, that actually drives the motor, is stepper.runSpeed(). You call it exactly once at the end of the loop() function. Though that function is designed to be called very often, as it only checks, if it is time to do a step, and then do so. It just an do max 1 step per execution. So you get 1 step per loop() iteration.

And at the start of the loop() you wait for the MPU data to be ready with this line:

while (!mpuInterrupt && fifoCount < packetSize)

So you do the steps at max with the same rate, that you read the MPU. That seems to be not enough for your setup.

You can try calling the runSpeed() function more often (and with that removing the limit on the steps). Either rewrite your code to only do the MPU communication, if the MPU data is ready, and proceed with the other code otherwise. Or you can insert the runSpeed() function into the while loops, that are waiting for the MPU data. The first option would be cleaner and would make the code easier to extent.