Self Balancing Robot

Hi Friends,
I m focused on making a self Balancing Robot and eventually i made one.

Here is a short video of the robot.

Here is my code. I find the codes from web and modified for myself.

#include "Wire.h"
#include "SPI.h"  
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//#define Gry_offset 0 
//#define Gyr_Gain 0.00763358  
#define Angle_offset 0
//#define RMotor_offset 30
//#define LMotor_offset 30
#define pi 3.14159 

//long data;
//int x, y;
float kp, ki, kd; 
float r_angle, f_angle, omega;
//float Turn_Speed = 0, Turn_Speed_K = 0;
//float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;

unsigned long preTime = 0;
//float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr, pTerm, iTerm, dTerm;
int timeChange; 

int TN1=4;
int TN2=3;
int ENA=9;

int TN3=6;
int TN4=5;
int ENB=10;

void setup() {
  Wire.begin();
  accelgyro.initialize();
  pinMode(TN1,OUTPUT);
  pinMode(TN2,OUTPUT);
  pinMode(TN3,OUTPUT);
  pinMode(TN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  

  Serial.begin(115200);
}

void loop() {

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  r_angle = (atan2(ax, az) * 180 / pi + Angle_offset);
  omega = (double) gy/131;  
  
    myPID();
    PWMControl();
}



void myPID(){
  kp = map(analogRead(A2),0,1023,0,5000)*0.1; 
  ki = map(analogRead(A0),0,1023,0,5000)*0.0001; 
  kd = map(analogRead(A1),0,1023,0,10000);  
   //  kp=30.50  kd=532.00  ki=0.31
   //kp=67.40;  
   //kd=488.00 ; 
   //ki=2.40;

   Serial.print("  kp=");Serial.print(kp);
  Serial.print("  kd=");Serial.print(kd);
   Serial.print("  ki=");Serial.print(ki);
   
  unsigned long now = millis();
  float dt = (now - preTime) / 1000.0;
  preTime = now;
  f_angle = (1-dt) * (f_angle + omega * dt) + dt * r_angle; 

  Serial.print("  angle=");Serial.println(f_angle);
  timeChange = (now - lastTime);
  
     
    error = 2.60-f_angle;
    
    //errSum += error * timeChange; 
    //if (abs(errSum)>500){errSum=500;}
    dErr = (error - lastErr); 
    pTerm=kp*error;
    
    //iTerm=ki*errSum;
    iTerm=constrain(iTerm,-200,200);
    iTerm+=error;
     //if (f_angle==0){iTerm=0;} 
    //iTerm=constrain(iTerm,-255,255);
    dTerm=kd*dErr;
    Output = pTerm+ki*iTerm+dTerm;
    if (Output<=-255)
    {
      Output=-255;
    }
    else if(Output>=255)
    {
      Output=255;
      
    }
    /*if (Output<0)
    {
      LOutput = Output + (-10);  
      ROutput = Output + 10;
    }
    else if (Output>0)
    {
      LOutput = Output + 10; 
      ROutput = Output + (-10); 
    }*/
    LOutput = Output;  
    ROutput = Output;
    
    lastErr = error;
    lastTime = now;
  
}

void PWMControl(){
  if(LOutput < 0){
    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, LOW);
  }
  else if(LOutput > 0){
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, HIGH);
  }
  else{
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, LOW);
  }
  if(ROutput < 0){
    digitalWrite(TN3, HIGH);
    digitalWrite(TN4, LOW);
  }
  else if(ROutput > 0){   
    digitalWrite(TN3, LOW);
    digitalWrite(TN4, HIGH);
  }
  else{
    digitalWrite(TN3, LOW);
    digitalWrite(TN4, LOW);
  }
    analogWrite(ENA, min(255, abs(LOutput)));
    analogWrite(ENB, min(255, abs(ROutput)));
}

I used 500 rpm 25mm motors without encoders. MPU6050, 3 potentiometer, Arduino UNO and motor driver.

I spent hours to find the best pid values but unfortunately i couldnt. The best result of my robot is shown on the video. Normally these robots oscilates a few times and and the oscilation periods get smaller each period. But in my project it is getting higher by the time.

As a result, I couldnt find what's wrong with my robot.
Help me please if you have an dea about this problem. Is this problem comes from chasis?, or from motor torque or rpm?, or codes? or PID?, or complementary filter? ... etc.
Thanks for helps :slight_smile:

Hey,
I did a similar project and had the same issue. I managed it by skipping the Integral polynom and adding a PD-position controller (with rotary encoder) which controlls the setangle of the PD-angle controller. Morover, I use a fixed controllerloop iteration time (45ms for the position controller & 15ms for the angle control).
The key to stabilisation for me was skipping the integralangle and adding a robot speed controller (D controller of position) which trys to keep the robot at v=0.

Unfortunately i started with a new project and disassembled my segway robot, but if you wish i can send you some code :wink:

Good Luck and Merry Christmas
Magnus Gärtner

magnusgaertner
Thank you very much for your answer. It is a good idea fixing loop time. I haven't thought that. I am also planning to change motors 300 rpm encodered (it's torque will increase and it will move in low pwm pulses. I hope :o ). I spent hours for pid values and tried lots of values for pid as kp, ki, and kd. I also set ki to zero and set just kp and kd but the result is failure :(.
Anyway,
Now my plan:
1- Try to fixing loop time according to your suggest.
2- Try to get smaller the height of the robot.
3- Try to set kp ki kd again & again :slight_smile:
4- Try to use new motors as i mentioned above. (By the way, I dont know how to code the encoder pulses and how to use the datas in pid formule. I will make research for it too).

It would be thankful to share your code.
Merry Christmas :slight_smile:
Burak

Sorry for all the variable names, i just copy pasted the balance routine out of my project

void driveBalanced(int RcRight1,int RcLeft1)
{
 if(time1+1000<micros())//updates Angel (a, deriv is da)
  {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    aAcc=-57.2957*atan((double)ax/az);
    (az<0&&ax>=0)?aAcc-=180:true;
    (az<0&&ax<0)?aAcc+=180:true;
    da=(double)(gy+aOffset)*0.00077157;
    aGyro+=da*(micros()-time1)/100000;
    a=0.999*(a+da*(micros()-time1)/100000)+(double)0.001*aAcc;
    time1=micros();
  }

   if(time15+15<millis())
   {
     time15=millis();
     RcPosLeft+=(double)RcLeft1*7/100;
     RcPosRight+=(double)RcRight1*7/100; 
     aDiffL=balancePointL-a;
     aDiffR=balancePointR-a;
     PIDOutL=(double)Kp*aDiffL-(double)Kd*(a-lasta);
     PIDOutR=(double)Kp*aDiffR-(double)Kd*(a-lasta);
     lasta=a;
     
    output(PIDOutL,PIDOutR);
   }
   
  if(time45+45<millis())
   {
     time45=millis();
     SpeedL=(double)(_LeftEncoderTicks-lastTachoA-RcLeft1/4)*val[3]/1000;
     SpeedR=(double)(-(_RightEncoderTicks-lastTachoB)-RcRight1/4)*val[3]/1000;
      if(abs(RcRight1-lastRcRight)>20)
      {
        RcPosRight=-_RightEncoderTicks;
      }
      if(abs(RcLeft1-lastRcLeft)>20)
      {
        RcPosLeft=_LeftEncoderTicks;
      }
     lastRcRight=RcRight1;
     lastRcLeft=RcLeft1; 
     posL=(double)(_LeftEncoderTicks-RcPosLeft)*val[4]/10000;
     posR=(double)-(_RightEncoderTicks+RcPosRight)*val[4]/10000;
     balancePointL=aBias+(double)0.6-SpeedL-posL;
     balancePointR=aBias+(double)0.6-SpeedR-posR;

     lastTachoA=_LeftEncoderTicks;
     lastTachoB=_RightEncoderTicks;
     
    
   }
     
  
}

Thank you very much for sharing the code. Nowadays i'm a little bit busy. so, i can not try it. Thanks for your help and code. i will try it ASAP.

magnusgartner,
after a long time, i checked your code but actually i couldn't understand.

Friends,
I made research on the web but i couldnt find where is my problem. But i suspect that my problem is in complementary filter and maybe loop time. When i tilt the robot in any time i saw that the angle values delay. I mean when moving the values change and then i stabilise it manually in the upright position angle counts down with delay in a few seconds.

So, i m sharing again my code, can anybody help me please :slight_smile:

#include <MPU6050_6Axis_MotionApps20.h>

//Jeff Rowberg's library

#include <I2Cdev.h>

#include "Wire.h"
#include "SPI.h"  


MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//#define bekle 0.02

#define ofset_aci 0 // Angle offset
#define pi 3.14159 
#define Angle_offset 0

float r_angle, f_angle, omega;
unsigned long lastTime;
unsigned long preTime = 0;
int timeChange,timeDiff;
unsigned long loopPreTime = 0;
unsigned long loopLastTime;

float kp, ki, kd;
float error,pTerm,integrated_error,iTerm,dErr,Output,lastErr;


#define motor1 7
#define motor2 8
#define calisma 4 //motor start
#define motor1pwm 9
#define motor2pwm 10

float LOutput,ROutput;

void setup() {
   Wire.begin();
  accelgyro.initialize();

  Serial.begin(115200);

  pinMode(motor1,OUTPUT);
  pinMode(motor2,OUTPUT);
  pinMode(motor1pwm,OUTPUT);
  pinMode(motor1pwm,OUTPUT);
  pinMode(calisma,OUTPUT);
  digitalWrite(calisma,HIGH);
}

void loop() {

//Duration Time For Looping Block

  unsigned long loopTimeStart=millis(); 

  float loopdtime = (loopTimeStart-loopPreTime)/1000.00;

  loopPreTime=loopTimeStart;

  timeDiff=loopTimeStart-loopLastTime;

//Duration Time For Looping Block



  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

//MPU6050 accelerometer and gyro datas && Complementary Filter Block

  r_angle = (atan2(ax, az) * 180 / pi + Angle_offset);

  omega = (double) gy/131;  //In order to have the degree per sec

  f_angle = 0.98 * (f_angle + (omega * loopdtime)) + (0.02 * r_angle); 

  Serial.print("  aci=");Serial.print(f_angle);

//MPU6050 accelerometer and gyro datas && Complementary Filter Block



// kp ki kd Block, i used 3 potentiometers

  kp = map(analogRead(A2),0,1023,0,10000)*0.01; 

  ki = map(analogRead(A0),0,1023,0,10000)*0.01;

  kd = map(analogRead(A1),0,1023,0,10000)*0.01; 

  Serial.print("  kp=");Serial.print(kp);
  Serial.print("  kd=");Serial.print(kd);
  Serial.print("  ki=");Serial.print(ki);

// kp ki kd Block 



  myPID();
  PWMControl();
  loopLastTime=loopTimeStart;
  
}

void myPID()
{

//Duration Time For PID

  unsigned long now = millis();
  float dt = (now - preTime) / 1000.0;
  preTime = now;
  timeChange = (now - lastTime);
  Serial.print("  dt=");Serial.print(dt);

//Duration Time For PID


  
//PID Block

  error = ofset_aci - f_angle;

  pTerm=kp*error;

  integrated_error+=error;

  iTerm=ki*constrain(integrated_error,-100,100);

  dErr = (error - lastErr);

  Output = pTerm+iTerm+kd*dErr;

//PID Block 


  if (Output<=-255)
  {
    Output=-255;
  }
  else if(Output>=255)
  {
    Output=255;  
  }
  Serial.print("  Output=");Serial.println(Output);
  lastErr = error;
  lastTime = now;
}

// Motor Control Block
void PWMControl(){
  if(Output >  ofset_aci){
    digitalWrite(motor1, LOW);
    digitalWrite(motor2, LOW);
  }
  else if(Output < ofset_aci){
    digitalWrite(motor1, HIGH);
    digitalWrite(motor2, HIGH);
  }
  else{
    digitalWrite(calisma,LOW);
  }
  if(Output > ofset_aci){
    digitalWrite(motor1, HIGH);
    digitalWrite(motor2, HIGH);
  }
  else if(Output < ofset_aci){   
    digitalWrite(motor1, LOW);
    digitalWrite(motor2, LOW);
  }
  else{
    digitalWrite(calisma,LOW);
  }
    analogWrite(motor1pwm, min(255, abs(Output)));
    analogWrite(motor2pwm, min(255, abs(Output)));
}
// Motor Control Block

sorry for my poor English.