Self Balancing Bot with PID

In my previous project, I dealt with self-balancing robot without PID controller in this version of embedded the PID control on the same boat and the results are a better than the previous version obviously I am using the control systems part where our main objective is to achieve a critically damped robot. It should reach the steady state as it should have fewer oscillations and it should also recover from light to medium push.

In my previous bot without PID, I had to face problems where the robot will run away with a certain angle. Let's said it x and the Motors will move forward to balance the boat, however, the force or better say pseudo force that acts on the bot will be equal to the force that is pulling the robot downloads at a certain time. When these two forces balance the Robot full neither fall down nor it attain its vertical position. In this Mod, the robot will keep moving forward and forward in forward and finally, it will fall down
Obviously controlling this robot without PID has drawbacks, therefore, I worked on PID and the results are pretty good now.

The construction is same as it was in the previous Bot so if you have made the boat using my previous post then, of course, this will also work. All you have to do is to change the code and will be as good as new.

One thing you must remember while tuning the PID is, first of all, you must set everything parameters to zero that means P is equal to zero I is equal to zero and similarly is D is equal to zero. The Proportional part deals with the force part. It describes the force of your robot for any tilt i.e how vigorously it's gonna react.

So, first of all, you have to keep increasing your P-value until your oscillating. It may even oscillate wildly or it may even oscillate like a drowsy lump. All you have to do is to make the boat oscillate. After that increase the Derivative value to reduce these oscillations. Your robot will stand a bit still not exactly still at the center point without oscillating much. Largest the value of D, quicker it will attain the steady state. Now increase the I value. I value will deal with the response to the tilt. It will tell the bot how quickly you want your bot to recover from a tilt.

The circuit for the bot is same as the previous one.

Working 

Working Updated

*Latest Updated* Source_Code

	#include <PID_v1.h>
	#include <Wire.h>
	const int MPU_addr=0x68;
	double AccelX,AccelY,AccelZ,Tmp,GyroX,GyroY,GyroZ; //These will be the raw data from the MPU6050.
	uint32_t timer; //it's a timer, saved as a big-ass unsigned int. We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
	double Angle_X, Angle_Y; //These are the angles in the complementary filter
	#define degconvert 57.29577951 //there are like 57 degrees in a radian.
	#define m1_left 12
	#define m1_right 10
	#define m1_en 11
	#define m2_left 3
	#define m2_right 4
	#define m2_en 9
	double Kp = 20;
	double Kd = 0.005;
	double Ki = 120;
	double targetAngle = 0;
	double input, output, outputx;
	volatile int control = 0;
	volatile float currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
	PID pid(&input, &output, &targetAngle, Kp, Ki, Kd, DIRECT);
	int count = 0;
	void setup() {
	pinMode(m1_left, OUTPUT);
	pinMode(m1_right, OUTPUT);
	pinMode(m2_left, OUTPUT);
	pinMode(m2_right, OUTPUT);
	pinMode(m2_en, OUTPUT);
	pinMode(m1_en, OUTPUT);
	Wire.begin();
	#if ARDUINO >= 157
	Wire.setClock(400000UL); // Set I2C frequency to 400kHz
	#else
	TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
	#endif
	Wire.beginTransmission(MPU_addr);
	Wire.write(0x6B); // PWR_MGMT_1 register
	Wire.write(0); // set to zero (wakes up the MPU-6050)
	Wire.endTransmission(true);
	Serial.begin(57600);
	delay(100);
	//setup starting angle
	//1) collect the data
	Wire.beginTransmission(MPU_addr);
	Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
	Wire.endTransmission(false);
	Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
	AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
	AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
	AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
	Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
	GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
	GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
	GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
	//2) calculate pitch and roll
	double roll = atan2(AccelY, AccelZ)*degconvert;
	double pitch = atan2(-AccelX, AccelZ)*degconvert;
	//3) set the starting angle to this pitch and roll
	double gyroXangle = roll;
	double gyroYangle = pitch;
	double Angle_X = roll;
	double Angle_Y = pitch;
	input = Angle_Y-85+170 ; //For a net 0 degree inclination
	//start a timer
	timer = micros();
	pid.SetMode(AUTOMATIC);
	pid.SetSampleTime(5);
	pid.SetOutputLimits(-820, 820); // To keep the bot smooth when inclination is very small. It prevents jerks
	delay(2000);
	}
	void loop(){
	count++;
	Wire.beginTransmission(MPU_addr);
	Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
	Wire.endTransmission(false);
	Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
	AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
	AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
	AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
	Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
	GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
	GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
	GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
	double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
	timer = micros(); //start the timer again so that we can calculate the next dt.
	double roll = atan2(AccelY, AccelZ)*degconvert;
	double pitch = atan2(-AccelX, AccelZ)*degconvert;
	double gyroXrate = GyroX/131.0;
	double gyroYrate = GyroY/131.0; //Change 131 if you want a faster change or a slower one
	Angle_X = 0.99 * (Angle_X + gyroXrate * dt) + 0.01 * roll; // Calculate the angle using a Complimentary filter
	Angle_Y = 0.99 * (Angle_Y + gyroYrate * dt) + 0.01 * pitch;
	currentAngle = Angle_Y-85+170;
	input = currentAngle;
	if(currentAngle > targetAngle-0.15 && currentAngle < targetAngle+0.15)
	stop();
	else {
	errorSum = currentAngle - prevAngle;
	targetAngle = (targetAngle - errorSum)*1.00000001;
	error = currentAngle - targetAngle;
	pid.SetTunings(Kp, Ki, Kd);
	pid.Compute();
	if(output>255)
	output = 255;
	else if(output<-255)
	output = -255;
	if(output < 0 && currentAngle < 30){
	command(false, output);
	}
	else if(output > 0 && currentAngle > -30)
	command(true, output);
	else{
	stop();
	count = 0;
	}
	prevAngle = currentAngle;
	}
	Serial.println(currentAngle);
	}
	void command(bool flag, int pwm) {
	if(flag == true) {
	analogWrite(m2_en, pwm);
	analogWrite(m1_en, pwm);
	digitalWrite(m2_left, LOW);
	digitalWrite(m2_right, HIGH);
	digitalWrite(m1_right, LOW);
	digitalWrite(m1_left, HIGH);
	}
	else {
	analogWrite(m2_en, abs(pwm));
	analogWrite(m1_en, abs(pwm));
	digitalWrite(m2_left, HIGH);
	digitalWrite(m2_right, LOW);
	digitalWrite(m1_right, HIGH);
	digitalWrite(m1_left, LOW);
	}
	}
	void stop(){
	digitalWrite(m2_right, LOW);
	digitalWrite(m1_right, LOW);
	digitalWrite(m2_left, LOW);
	digitalWrite(m1_left, LOW);
	}

view raw Self Balancing Robot_Latest.ino hosted with ❤ by GitHub

Ping me if you face any problem. PID tuning takes luck and not the time I guess. In my next post, I'll deal with PID automatic tuning for this bot. So Long.