# Bug in code of Self-Balancing Bot

I am making a Self balancing bot as my project in which I am using MPU6050, Arduino Uno, motor driver l298n and DC motors. I am making this using data from accelerometer and gyroscope and combining them through complementary filter, after that using PID and error of angle made by bot to get the PWM required for DC motor. 1) The values of PID and angles resultant of complementary filter drifts to the extreme every time I run this which I guess shouldn't happen but the values of accelerometer and gyro(without using filter) gives me approx values , I guess this is due to my mistake in the calculation of angles or in the filter that I have applied which I have been trying to but can't find it.

2) I have tried to calibrate MPU6050 many times but failed as the x-axis value is showing 0 only when I tilt the bot to 12-14 degrees on x-axis(data from only accelometer),(if you can just tell me the variable to modify).

#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ,elapsedTime;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
double x,y,z,dx,dy,dz,ex,ey,ez,desx=0,desy=0,desz=0;
float PID, PWM, error, previous_error,timePrev,time1;
float pid_p;
float pid_i;
float pid_d;
/////////////////PID CONSTANTS/////////////////
double kp=2;
double ki=0;
double kd=0;
int motor1pin1 = 2;
int motor1pin2 = 3;
int led = 9;           // the PWM pin the LED is attached to
int brightness = 0;    // how bright the LED is

void setup()
{
Serial.begin(115200);
Wire.begin();
pinMode(motor1pin1, OUTPUT);
pinMode(motor1pin2, OUTPUT);
pinMode(9, OUTPUT);
pinMode(13, OUTPUT);
setupMPU();
delay(500);
}

void loop()
{
timePrev = time1;  // the previous time is stored before the actual time read
time1 = millis();  // actual time read/
elapsedTime = (time1 - timePrev) / 1000;
recordAccelRegisters();
recordGyroRegisters();
processangle();
Apply_PID();
printData();
delay(100);
}
void right(int PWM)
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
digitalWrite(13, HIGH);
Serial.println("RIGHT");
}
void left(int PWM)
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
Serial.println("LEFT");
}
void stopper()
{
analogWrite(9, PWM);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
Serial.println("STOPPED");
}
void setupMPU()
{
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}

void recordAccelRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
while(Wire.available() < 6);
processAccelData();
}

void processAccelData()
{
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
z= RAD_TO_DEG * (atan(sqrt(square(-gForceY) + square(-gForceX)) / -gForceZ));
/* x=map(x,340,11,-90,90);*/

Serial.print(" AccelX=");
Serial.print(x);

}

void recordGyroRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
processGyroData();
}

void processGyroData()
{
rotX = (gyroX / 131.0);
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
Serial.print(" GyroX=");
Serial.println(rotX*elapsedTime);
}
void processangle()
{
dx=0.98 *(dx + rotX*elapsedTime) + 0.02*x;
dy=0.98 *(dy + rotY*elapsedTime) + 0.02*y;
dz=0.98 *(dz + rotZ*elapsedTime) + 0.02*z;
ex=dx-desx;
ey=dy-desy;
ez=dz-desz;
Serial.print("After Filter Error");
Serial.println(ex);
}
void Apply_PID()
{
float l=(float)ex;
pid_p=kp*l;
pid_i = pid_i+(ki*l);
pid_d = kd *((l - previous_error)/elapsedTime);
PID = pid_p + pid_i + pid_d;
/*Serial.print("I=");
Serial.println(pid_d);
Serial.print("p=");
Serial.println(pid_p);*/
Serial.print("PID");
Serial.println(PID);

if(l>5.5 || l<5.5)
{
if(l<0)
right(PID);
else
left(PID);
}
else
{
stopper();
}
previous_error = l;
}

void printData()
{
/*Serial.print("Gyro (deg)");
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.print(rotY);
Serial.print(" Z=");
Serial.print(rotZ);
Serial.print(" Accel (g)");
Serial.print(" X=");
Serial.print(gForceX);
Serial.print(" Y=");
Serial.print(gForceY);
Serial.print(" Z=");
Serial.println(gForceZ);*/
/* Serial.print(" PWM");
Serial.println(PID);*/
}

• Welcome to Robotics Aditya Raj, but I'm afraid that it is near impossible for us to debug your code for you without a lot more information. Please include details of what you want to achieve, what you tried, what you saw & what you expected to see. Details of the physical setup, sensor arrangement, and code output are also crucial.
– Ben
Aug 11 at 20:12
• We prefer practical, answerable questions based on actual problems that you face. Take a look at How to Ask and tour for more information on how stack exchange works. Also, the Robotics question checklist has good advice on how to write a good question. Please edit your question with additional detail.
– Ben
Aug 11 at 20:12
• Have changed explanation of my problem for better understanding.... Aug 11 at 21:40

There's a lot going on here, and the issues aren't very clear to me. I would suggest trying to tackle your problems one at a time, and to not move on until you are confident the root issues are sorted out. For example, you have trouble with your PID output, but you also have issues with your angle estimates. You'll never fix the PID controller until you get your estimates correct. But you're also having issues with your calibration, in what sounds like maybe a data processing or register reading issue, and again you'll never get your estimates correct until you get your data handling fixed.

I would urge anyone working on a project to use as many existing libraries as possible - don't spend your time reinventing the wheel unless that is what your project is supposed to do. Your question here is titled, "Bug in code of self-balancing bot," so I would suggest you use the existing libraries for the IMU, use a library for processing the IMU, and then that frees you to focus solely on the self-balancing robot.

There is an MPU6050 library at this Adafruit tutorial, and I always highly recommend the Madgwick filter for converting IMU outputs to pose estimates. The page I linked for the Madgwick filter has implementations in C, C#, and Matlab.

That said, I can't tell what exactly the problems are that you're facing. When you say

the x-axis value is showing 0 only when I tilt the bot to 12-14 degrees on x-axis(data from only accelometer)

I don't know what that means. It looks like you're trying to estimate angles from the accelerometers and trying to filter angular rates from the gyros. Are you meaning the angle about x is zero? The x gyro is zero? The x accelerometer is zero? I can't tell. Which way do you have the IMU oriented? When you say you're tilting the bot on x-axis, do you mean you're turning it about the x-axis? About some other axis?

Please give the MPU6050 library a try and see if that at least fixes your x-axis value showing zero. If you can get that working for you then maybe you can keep on with your complimentary filter and can get the rest of your code sorted out.

Once you get the readings coming in as expected, if your filtering doesn't work out, give the Madgwick filter a shot. That should give you very, very stable and accurate orientation estimates but, as I mentioned at the top of this, it'll never work unless you're able to get the readings off the chip correctly.