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;
float rad_to_deg = 180/3.141592654;
/////////////////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
int fadeAmount = 5;
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);
accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processAccelData();
}
void processAccelData()
{
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
x= (RAD_TO_DEG * (atan2(-gForceY, -gForceZ)+PI));
y= RAD_TO_DEG * (atan2(-gForceX, -gForceZ)+PI);
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);
gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
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);*/
}
