Need help in Self balancing code?


I have a created a beginner self balancing robot code by referring to this thread,,

Now I have built a robot which oscillates for probably three seconds and then it moves either forward continously or reverse and it falls, I need it to stabilize, Details are given below

2 x 10kg-cm 500 rpm motors
1 x l298 motor driver
1 x Arduino Uno
1 x gy 80 IMU(for accelerometer and gyroscope)
and I wish to stabilize it without using any kind of encoders.

here is my code, please make it free from errors

 #include <Wire.h>

#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define DEVICE (0x53)    //ADXL345 device address
#define TO_READ (6)

float Kp=0.1,Ki=0.5,Kd=1;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;

float initialmotorspeed= 60;
int xangle;
int yangle;
int zangle;
int a1, b1;
int STD_LOOP_TIME = 9;             
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

int L3G4200D_Address = 105; //I2C address of the L3G4200D

int a;
int b;
int c;
//num of bytes we are going to read each time (two bytes for each axis)

byte buff[TO_READ] ;    //6 bytes buffer for saving data read from the device
char str[512];                      //string buffer to transform data before sending it to the serial port

void setup()
  Wire.begin();        // join i2c bus (address optional for master)
  Serial.begin(115200);  // start serial for output
 pinMode(9,OUTPUT); //PWM Pin 1 TO 1 OF L293D
 pinMode(10,OUTPUT);//PWM Pin 2 to 9 of l293d
 pinMode(2, OUTPUT);//2 pin of l293d
 pinMode(3, OUTPUT);//7 pin of l293d 
 pinMode(4, OUTPUT);//10 pin of l293d
 pinMode(5, OUTPUT);//15 pin of l293d
  setupL3G4200D(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec

  //Turning on the ADXL345
  writeTo(DEVICE, 0x2D, 0);      
  writeTo(DEVICE, 0x2D, 16);
  writeTo(DEVICE, 0x2D, 8);

void loop()
  getGyroValues();  // This will update x, y, and z with new values
  int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
  int x, y, z;
  readFrom(DEVICE, regAddress, TO_READ, buff); //read the acceleration data from the ADXL345
   //each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
   //thus we are converting both bytes in to one int
  x = (((int)buff[1]) << 8) | buff[0];   
  y = (((int)buff[3])<< 8) | buff[2];
  z = (((int)buff[5]) << 8) | buff[4];

  float x1 = atan(x/sqrt(pow(y,2) + pow(z,2)));
  float y1 = atan(y/sqrt(pow(x,2) + pow(z,2)));
  float z1 = atan(z/sqrt(pow(x,2) + pow(y,2)));
  xangle = (0.98*(xangle + c*0.01) + (0.02*x1));
  yangle = (0.98*(yangle + b*0.01)+ (0.02*y1));
  zangle = (0.98*(zangle + a*0.01) + (0.02*z));
  Serial.print("Xangle: ");
  Serial.print("Yangle: ");
  Serial.print("Zangle: ");

  Serial.print("error: ");
 Serial.print("PID VALUE: ");
 Serial.print("b1: ");
 lastLoopUsefulTime = millis()-loopStartTime;
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
 //delay function will have delay of 100hz

 //3 functions for executing pid


float motorcontrol()
  if (error > 0)
  if (error < 0)
     PID_value = -PID_value;
  if(error == 0)

float calculatepid()
  error = 0 + yangle;
  P = error;
  I = I + previous_I;
  D = error-previous_error;
  PID_value = (Kp*error) + (Ki*I) + (Kd*D);
    return PID_value;

void writeTo(int device, byte address, byte val) {
   Wire.beginTransmission(device); //start transmission to device 
   Wire.write(address);        // send register address
   Wire.write(val);        // send value to write
   Wire.endTransmission(); //end transmission

void getGyroValues(){

  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  a= ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  b = ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  c = ((zMSB << 8) | zLSB);

int setupL3G4200D(int scale){
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  if(scale == 250){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
  }else if(scale == 500){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);

void writeRegister(int deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.write(address);       // send register address
    Wire.write(val);         // send value to write
    Wire.endTransmission();     // end transmission

int readRegister(int deviceAddress, byte address){

    int v;
    Wire.write(address); // register to read

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) {
        // waiting

    v =;
    return v;

//reads num bytes starting from address register on device in to buff array
void readFrom(int device, byte address, int num, byte buff[]) {
  Wire.beginTransmission(device); //start transmission to device 
  Wire.write(address);        //sends address to read from
  Wire.endTransmission(); //end transmission
  Wire.beginTransmission(device); //start transmission to device
  Wire.requestFrom(device, num);    // request 6 bytes from device
  int i = 0;
  while(Wire.available())    //device may send less than requested (abnormal)
    buff[i] =; // receive a byte
  Wire.endTransmission(); //end transmission

int motorforward()
  a1 = initialmotorspeed + PID_value;
  b1 = constrain(a1, 0, 255);
   analogWrite(9, b1);
  analogWrite(10, b1);
  digitalWrite(2, HIGH);
  digitalWrite(3, LOW);
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
  return b1;
int motorreverse()
  a1 = initialmotorspeed + PID_value;
  b1 = constrain(a1, 0, 255);
  analogWrite(9, b1);
  analogWrite(10, b1);
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH);
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
  return b1;
void motorstop()
  analogWrite(9, 0);
  analogWrite(10, 0);
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);

Does your robot not work at all, or just not work very well ?

I am not sure, it oscillates for about 3 seconds then it loses its balance and falls, I suppose its working but I have never seen it standing still, everytime it oscillates

Your PID coefficients look… well, really wrong. In most cases, Kp will be larger than Ki, and Ki will be larger than Kd. What you have is exactly the opposite, and I would EXPECT oscillation and very unstable behavior. You almost certainly don’t need Kd at all, so set that to 0. Start with Ki at 0 as well, and see what it does with only Kp set. If it’s at least stable, increase Kp and test again. Keep increasing Kp until it starts to oscillate, then back off a bit on Kp and add a SMALL amount of Ki. I woudl expect Ki to want to be no more than 10-25% of Kp.

Ray L.

This is my self balancing robot

Additional info:

I have calculated x, y, and z, angles using complementary filters, but used only y because, it’s the one which changes when robot oscillates, I am not sure whether it is right?

I know I should use 2 angles and 1 angular rate, not sure how to use filters for this. tried but in vain. Tried with complementary filter from this website "" but I don't understand really