tuning pid for 2 wheel self balancing robot

I am a student of delhi university and i could not find Kp,Ki and Kd values for my 2 wheeled self balancing robot.

I have designed a 2 wheel self balancing robot that is arduino based and uses mpu6050 gyro+accelerometer to read the tilt angle of the self balancing robot. Also i have used PID control system also i have tried a lot of hit and trial to find Kp, Ki and Kd values but couldn't find an accurate set of values. Also i cannot determine transfer function fo my self balanced robot so i cannot use that simulink model for tuning pid values from pid tuner in matlab.

So plz help me to find out an accurate Kp,Ki and Kd values to find an exact pid values to balance my self balancing robot.

Also there is no error in code and any kind

I have shared code used in arduino and Kp, Ki and Kd values as best as possible from hit and trial method

CODE USED IN ARDUINO AS FOLLOWS:

#include <Wire.h>

//Gyro Variables

float elapsedTime_mpu, time_mpu, timePrev_mpu; //Variables for time control

int gyro_error=0; //We use this variable to only calculate once the gyro data error

float Gyr_rawX, Gyr_rawY, Gyr_rawZ; //Here we store the raw data read

float Gyro_angle_x, Gyro_angle_y; //Here we store the angle value obtained with Gyro data

float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error

//Acc Variables

int acc_error=0; //We use this variable to only calculate once the Acc data error

float rad_to_deg = 180/3.141592654; //This value is for pasing from radians to degrees values

float Acc_rawX, Acc_rawY, Acc_rawZ; //Here we store the raw data read

float Acc_angle_x, Acc_angle_y; //Here we store the angle value obtained with Acc data

float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error

float Total_angle_x, Total_angle_y;

//variables for driving car

int pwmLeft=9, pwmRight=10, left1=3, left2=4, right1=5, right2=6;

int leftSpeed=0, rightSpeed=0;

int maxSpeed=60;

int Speed;

/////////////////PID/////////////////

float setpoint =0.2;

float elapsedTime, Time, timePrev;

float PID=0.00, error, previous_error;

float pid_p=0;

float pid_i=0;

float pid_d=0;

/////////////////PID CONSTANTS/////////////////

double kp=45;

double ki=0.025;

double kd=1.02;

void setup() {

Wire.begin(); //begin the wire comunication

Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)

Wire.write(0x6B); //make the reset (place a 0 into the 6B register)

Wire.write(0x00);

Wire.endTransmission(true); //end the transmission

//Gyro config

Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)

Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)

Wire.write(0x10); //Set the register bits as 00010000 (1000dps full scale)

Wire.endTransmission(true); //End the transmission with the gyro

//Acc config

Wire.beginTransmission(0x68); //Start communication with the address found during search.

Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register

Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)

Wire.endTransmission(true);

Serial.begin(115200); //Remember to set this same baud rate to the serial monitor

time_mpu = millis(); //Start counting time in milliseconds

Time=millis();

}//end of setup void

void loop() {

timePrev_mpu = time_mpu; // the previous time is stored before the actual time read

time_mpu = millis(); // actual time read

elapsedTime_mpu = (time_mpu - timePrev_mpu) / 1000; //divide by 1000 in order to obtain seconds

//////////////////////////////////////Gyro read/////////////////////////////////////

Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)

Wire.write(0x43); //First adress of the Gyro data

Wire.endTransmission(false);

Wire.requestFrom(0x68,4,true); //We ask for just 4 registers

Gyr_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum

Gyr_rawY=Wire.read()<<8|Wire.read();

/*Now in order to obtain the gyro data in degrees/seconds we have to divide first

the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/

/*---X---*/

Gyr_rawX = (Gyr_rawX/32.8);

/*---Y---*/

Gyr_rawY = (Gyr_rawY/32.8);

/*Now we integrate the raw value in degrees per seconds in order to obtain the angle

* If you multiply degrees/seconds by seconds you obtain degrees */

/*---X---*/

Gyro_angle_x = Gyr_rawX*elapsedTime_mpu;

/*---X---*/

Gyro_angle_y = Gyr_rawY*elapsedTime_mpu;

//////////////////////////////////////Acc read/////////////////////////////////////

Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)

Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX

Wire.endTransmission(false); //keep the transmission and next

Wire.requestFrom(0x68,6,true); //We ask for next 6 registers starting withj the 3B

/*We have asked for the 0x3B register. The IMU will send a brust of register.

* The amount of register to read is specify in the requestFrom function.

* In this case we request 6 registers. Each value of acceleration is made out of

* two 8bits registers, low values and high values. For that we request the 6 of them

* and just make then sum of each pair. For that we shift to the left the high values

* register (<<) and make an or (|) operation to add the low values.

If we read the datasheet, for a range of+-8g, we have to divide the raw values by 4096*/

Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres

Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;

Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ;

/*Now in order to obtain the Acc angles we use euler formula with acceleration values

after that we substract the error value found before*/

/*---X---*/

Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;

/*---Y---*/

Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_y;

//////////////////////////////////////Total angle and filter/////////////////////////////////////

/*---X axis angle---*/

Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;

/*---Y axis angle---*/

Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;

/*Uncoment the rest of the serial prines

* I only print the Y angle value for this test */

pid_compute();

drive_car();

// Serial.print(millis());

// Serial.print(" ");

Serial.println(error);

}

void pid_compute()

{

timePrev=Time;

Time=millis();

elapsedTime=Time-timePrev;

error = Total_angle_x - setpoint;

pid_p = kp*error;

if(error>-1 && error<1)

{

pid_i = pid_i+(ki*error);

}

pid_d = kd*((error - previous_error)/elapsedTime);

PID = pid_p + pid_i + pid_d;

if(PID < -255)

{

PID=-255;

}

else if(PID > 255)

{

PID=255;

}

previous_error = error;

Speed=abs(PID);

}

void drive_car()

{

if(error<0.5 && error>-0.5)

{

leftSpeed=0;

rightSpeed=0;

}

else

{

leftSpeed=Speed;

rightSpeed=Speed;

}

if(leftSpeed > 255) { leftSpeed=255;}

else if(leftSpeed < 0){ leftSpeed=0;}

if(rightSpeed > 255) { rightSpeed=255;}

else if(rightSpeed < 0){ rightSpeed=0;}

analogWrite(pwmLeft,leftSpeed);

analogWrite(pwmRight,rightSpeed);

if(error<-0.5)

{

//driveBack

digitalWrite(left1,LOW);

digitalWrite(left2,HIGH);

digitalWrite(right1,LOW);

digitalWrite(right2,HIGH);

}

else if(error>0.5)

{

//driveForth

digitalWrite(left1,HIGH);

digitalWrite(left2,LOW);

digitalWrite(right1,HIGH);

digitalWrite(right2,LOW);

}

}

[Raj]

Trial and error is a bad choice to estimate your controller gains specially when the system is highly unstable. If you give a Google search on "2 wheel self balancing robot" you will find a number of papers and articles which explain that a 2 wheel self balancing robot is equivalent to an inverted pendulum model. Again there are number of models of inverted pendulum on the web and infact MATLAB itself has a demo model for same. You can use the system model from there to estimate your PID gains.