Arduino Forum

Topics => Robotics => Topic started by: Cheetahunter on Feb 18, 2016, 07:45 am

Title: Self balancing robot
Post by: Cheetahunter on Feb 18, 2016, 07:45 am
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 ,?
Title: Re: Self balancing robot
Post by: MarkT on Feb 18, 2016, 01:52 pm
You have two control loops?
Title: Re: Self balancing robot
Post by: Cheetahunter on Feb 18, 2016, 03:24 pm
no.why?
Title: Re: Self balancing robot
Post by: jremington on Feb 19, 2016, 12:00 am
Post your code, using code tags ("</>" button).
Title: Re: Self balancing robot
Post by: MarkT on Feb 21, 2016, 12:45 pm
Well with one control loop you cannot control both balance and position.
Title: Re: Self balancing robot
Post by: Cheetahunter on Feb 25, 2016, 12:26 pm
here is my code, part1:

Code: [Select]
#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);

}





Title: Re: Self balancing robot
Post by: Cheetahunter on Feb 25, 2016, 12:31 pm
this is the rest of the code:
Code: [Select]
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;}
  
}
}
Title: Re: Self balancing robot
Post by: jremington on Feb 25, 2016, 05:24 pm
Do you have a theory for how that code is supposed to work?
Title: Re: Self balancing robot
Post by: Cheetahunter on Feb 25, 2016, 07:06 pm
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.
https://www.youtube.com/watch?v=PnIHfxEg3BI
Title: Re: Self balancing robot
Post by: jremington on Feb 25, 2016, 07:23 pm
Quote
my problem is with the movement.
Do you have a theory as to how "movement" is supposed to work?
Title: Re: Self balancing robot
Post by: Cheetahunter on Feb 25, 2016, 07:45 pm
I tried to make it move by lean angle.
But its not good enough.
Title: Re: Self balancing robot
Post by: jremington on Feb 25, 2016, 10:59 pm
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.
Title: Re: Self balancing robot
Post by: MarkT on Feb 26, 2016, 01:20 pm
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.