I am working on a proof-of-concept experiment that is using an MPU6050 (just to compute roll value) to drive a stepper motor with PID control (roll value as input, speed of stepper motor as output). I am using the Electronic Cats MPU6050 library to get my roll value (runs with interrupts), and then the AccelStepper library to drive the stepper motor. Here is my issue:
When the roll value gets too far away from the PID setpoint, 0, my variable "roll" (computed in the interrupt) stops being the right value. If I comment out the line that actually drives the motor -"stepper.setSpeed(Output);" - at the bottom of my main loop, the issue goes away. So I suspect that setSpeed() or an associated function is somehow blocking my interrupt, and then the ISR is miscalculating roll because the time since the last ISR is inaccuate. However, I don't understand how this could happen because I thought the interrupt should always have priority over anything else that my code is doing. Please let me know if anyone has a suggestion for how to fix this or an explanation for why it isn't working.
Here is the Accelstepper documentation. I also attached my entire code. Let me know if any other information is needed. I am using an Uno. Thank you!
https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html
/////////////////////
// Reactionary moment arm proof of concept
// using Accelstepper, MPU6050, PID libraries
////////////////////
//stepper
#include <AccelStepper.h>
//IMU stuff
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
//PID
#include <PID_v1.h>
//stepper
AccelStepper stepper(2, 8,9);
unsigned long steptimer = 0;
unsigned long oldsteptimer = 0;
// MPU vars
MPU6050 mpu;
#define INTERRUPT_PIN 2
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
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
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
float pitch;
float roll;
//PID
double Pk = 10;
double Ik = 0;
double Dk = 1;
double Setpoint = 0.0, Input, Output;
PID pid(&Input, &Output, &Setpoint, Pk, Ik , Dk, REVERSE);
unsigned long t;
// ****************** SETUP ******************************
void setup() {
Serial.begin(115200);
pinMode(2, INPUT_PULLUP);
// IMU 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
Wire.setWireTimeout(3000,true);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize device
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(177);
mpu.setYGyroOffset(-30);
mpu.setZGyroOffset(-89);
mpu.setXAccelOffset(-4105);
mpu.setYAccelOffset(-2437);
mpu.setZAccelOffset(507);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
//Serial.println("bad");
}
pid.SetMode(AUTOMATIC);
pid.SetOutputLimits(-1000, 1000);
pid.SetSampleTime(10);
stepper.setMaxSpeed(1000);
} // end of setup
// ********************* MAIN LOOP *******************************
void loop() {
if (!dmpReady) return;
//calc roll
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
roll = (ypr[2] * 180/M_PI);
Serial.print("roll: ");
Serial.println(roll);
}
Input = roll;
pid.Compute();
//Serial.print(" output: ");
//Serial.println(Output);
steptimer = millis();
if ((steptimer - oldsteptimer) > 10){
oldsteptimer = steptimer;
stepper.setSpeed(Output);
}
stepper.runSpeed();
}