Hi all,
I have built a self balancing robot using arduino Due and mpu6050. I managed to stabilize thr robot really good not with pid But by LQR method .
I have only problem with driving the robot.
When I set an angle to make the robot move forward it does go forward but the angle get bigger and bigger till it falls .
Can someone have an idea how to fix it ,?
You have two control loops?
no.why?
Post your code, using code tags ("</>" button).
Well with one control loop you cannot control both balance and position.
here is my code, part1:
#include <Encoder.h>
#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
char blueToothVal; //value sent over via bluetooth
char lastValue; //stores last state of device (on/off)
#endif
/* ===========================================================
* ========= MPU Definitions ==============================
* ===========================================================
*/
//Define MPU 6050 Variables
MPU6050 mpu;
int t1=0;
// 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
VectorInt16 aaOffset; // [x, y, z] accel sensor measurements offset
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float yprOffset[] = { 0, 0.15, 0 }; // [yaw, pitch, roll] offset yaw/pitch/roll container and gravity vector
float gyroOffset[] = {0, 0, 0}; //[x,y,z] offsets look for the axes on the MPU
int16_t gyro[3]; //[gx ,gy, gz] deg/sec
// INTERRUPT DETECTION ROUTINE
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
#define LED_PIN 13
float z=0.4;
bool blinkState = false;
/* ===========================================================
* ========= Encoder Definitions ==========================
* ===========================================================
*/
//State the Pins of the Encoder using Interrupt pins
Encoder myEnc(9, 10);
// Right wheel drive (Encoder working on Right Motor)
Servo victorRight;
Servo victorLeft;
//Intial Vars:
//Encoder Related var:
long POS1 = 0;
long POS2 = 0;
long POS3 = 0;
long POS4 = 0;
long POS5 = 0;
long oldPosition = -999;
long npo;
long newPosition = 0;
int xT = 0;
int sT = 0;
int dT = 0;
/*=========================================================
* ================== State Space ========================
* =========================================================
*/
float K[2][6] = { // Velocity via Encoders Only
// { -27, -2.707, -0.5, -100, -30, -4}, // 3-> -34.7573
//{ -27, -2.707, -0.5, -100, 30, 4 }
//{ -37, -1.707, -2, -10, -7, -2}, //good
//{ -37, -1.707, -2, -10, 7, 2 }
{ -47, -2.707, -1, -5, -30, -4}, //good
{ -47, -2.707, -1, -5, 30, 4 }
// { -50, -0.707, -1, -15, -15.2361, -2.3324}, // 3-> -34.7573
// { -50, -0.707, -1, -15, 15.2361, 2.3324 }
};
// Motor Voltage From State Space vars:
float Vr = 0;
float Vl = 0;
//State Reading Vector: [ theta, x, theta_dot, x_dot, psi, psi_dot]'
volatile float State_Readings[] = {0,0,0,0,0,0};
//Desired Velocity in Meter / Sec
float D_Velocity = 0;
//Desired Steer Angle in radians
float D_Steer = 0;
float D_Angle = 0;
void setup()
{
int npo=0;
Serial.begin(115200);
// Serial.begin(9600);
// Attach Pin to right motor
victorRight.attach(8);
victorLeft.attach(6);
pinMode(12,OUTPUT);
int t1=0;
// Mpu communications protocol
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//Mpu Setup
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// 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
// These Offset tested for each MPU, IMPORTANT STEP
mpu.setXGyroOffset(178);
mpu.setYGyroOffset(19);
mpu.setZGyroOffset(32);
mpu.setXAccelOffset(1022);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(1310);
// 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(0, 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(")"));
}
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
this is the rest of the code:
void loop()
{
if(Serial.available())
{//if there is data being recieved
blueToothVal=Serial.read(); //read it
}
switch (blueToothVal) {
// Controller commands:
case 'a':
if (D_Angle<0.6){
D_Angle =D_Angle +0.001;}
else
blueToothVal='k';
break;
case 'c':
D_Angle = 0;
blueToothVal='k';
break;
case 'e':
if (D_Angle>-0.45){
D_Angle =D_Angle -0.001;}
else
blueToothVal='k';
break;
case 'd':
if(D_Steer<1.2){
D_Steer =D_Steer+ 0.2;
blueToothVal='k';
}
break;
case 'b':
if(D_Steer>-2.8){
D_Steer =D_Steer- 0.2;
blueToothVal='k';
}
break;
case 'k':
if (D_Angle>0){
D_Angle =D_Angle- 0.0005;}
if (D_Angle<0){
D_Angle =D_Angle+ 0.0005;}
break;
default: 'k';
}
if(abs(D_Steer>1.6)){
D_Steer=0;
}
//if (abs(D_Velocity)>0.6){
// D_Velocity=0;
//}
//if(millis()-t1>1000){
// D_Velocity=0;
//}
xT = micros();
POS5 = POS4;
POS4 = POS3;
POS3 = POS2;
POS2 = POS1;
POS1 = myEnc.read();
newPosition = (POS1 + POS2 + POS3 + POS4 + POS5) / 5;
// Serial.println("-----------");
// Serial.println(newPosition);
if (sT >= 100){
State_Read();
for (int i = 0; i <= 5; i++){
//Serial.println(State_Readings[i]);
Vr += K[0][i] * State_Readings[i];
Vl += K[1][i] * State_Readings[i];
}
//Serial.println(State_Readings[0]);
// Serial.println("-----------");
// Serial.println(Vr);
// Serial.println(Vl);
// Serial.println("========");
int x = 24; //encoder = 80 24 shay
Vr = constrain(Vr, -x, x);
Vl = constrain(Vl, -x, x);
Vr = map(Vr, -x, x, 1000, 2000);
Vl = map(Vl, -x, x, 1000, 2000);
// Serial.println(State_Readings[0]);
victorRight.writeMicroseconds(Vr);
victorLeft.writeMicroseconds(Vl);
// Serial.println("---------");
// Serial.println(Vr);
// Serial.println(Vl);
// Serial.println("=========");
Vr = 0;
Vl = 0;
sT = 0;
}
oldPosition = newPosition;
dT = micros() - xT;
sT = sT + dT;
}
void State_Read()
{
// Encoders sensors
State_Readings[3] = (((newPosition - oldPosition) * 60) / (9.55* sT));
//State_Readings[1] += (State_Readings[3] * sT)/1000;
//State_Readings[1] = newPosition - oldPosition;
//MPU sensor
// 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;
// Get the Angular Velocitis in deg/sec
mpu.dmpGetGyro(gyro, fifoBuffer);
// Get Yaw Pitch Roll
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer); //Getting Acceleration
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
//Acceleration
// Accel = -aa.x*cos(ypr[1])*sT/1000;
//Velocity_MPU = Accel-((gyro[1]*M_PI*0.160)/180); //160 Distance from sensor to turning axe
// Set offset
ypr[0] -= yprOffset[0];
ypr[1] -= yprOffset[1];
ypr[2] -= yprOffset[2];
State_Readings[0] = ypr[1]- D_Angle;
State_Readings[2] = gyro[1] * M_PI / 180;
// State_Readings[3] = State_Readings[3]-D_Angle;
State_Readings[4] = ypr[0] - D_Steer;
State_Readings[5] = gyro[2] * M_PI / 180;
//Serial.println( Velocity_MPU); // Serial.print("\t"); Serial.print( State_Readings[1]); Serial.print("\t"); Serial.print( State_Readings[2]); Serial.print("\t"); Serial.print( State_Readings[3]); Serial.print("\t"); Serial.print( State_Readings[4]);
// Serial.println( State_Readings[4]);
// if (z>0){
// z=z-0.2;}
}
}
Do you have a theory for how that code is supposed to work?
sure. it works.
its using the LQR method (except the rc part).
there are gains calculated for each parameter.
my problem is with the movement.
my problem is with the movement.
Do you have a theory as to how "movement" is supposed to work?
I tried to make it move by lean angle.
But its not good enough.
No, that won't work. A constant lean angle requires a counter torque to oppose the torque due to gravity. In order to balance, a typical robot would have to constantly accelerate horizontally.
1 Like
A (slow) PID loop controlling the desired balance angle, fed from the position error, should be
providing the setpoint for the balance control loop. Well in theory.