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

• Angel Hidalgo: how about, did you manage to finish the project?

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.

• Frecka: That makes sense, thanks. My solution was to put all the MPU reading code inside the if (t_elapsed >= interval) block so that the sensor values are only read at set intervals and runSpeed() gets called every step. My cart speed is still slower than it is without the MPU (around 2/3 as fast), but it's much closer now. I'm still unsure how to get a smooth, fast speed -- I think my max right now is around 200 rpm, and setting the motor speed above around 1000 gives me trouble, but at least it's in the ballpark now.