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