FREE SHIPPING OVER \$79;

# Self-balancing Car Kit For Arduino Robot Project 11: Speed Loop Adjustment

Posted by Fiona Su on

Description:
We have introduced before that can adjust the angle to balance the car on the almost horizontal ground by controlling the PD.
However, if subjected to external forces or on a slope, the car can't keep balance because there is an error in speed. So you need to adjust the speed of the car for balancing.
Therefore, a speed measuring module is required.

The motor we used comes with a Hall encoder for speed measurement.
Based on the previous lesson, add a PI (proportional and integral) control to adjust the speed. In this way, it is possible to control the car to be balanced even if it is on a slope.
The speed loop here is positive feedback, which means that if the car is going backwards, then the car will move forward at a faster speed.
To be specific, if gently push the car with hand, the wheel of the car accelerates in the direction of pushing, that is, the direction in which the car is tilted, so that the tilt angle of the car is reversed in the opposite direction, that is, contrary to the direction of pushing; the car will accelerate in the direction of tilting, and the speed is accelerated.
The PI adjustment of speed just offsets the previously generated dip angle, and returns to near the initial equilibrium point without falling.

Principle and function of PI regulation:

; // speed loop control

Adjust the speed with the proportional parameter (kp_speed), so that the car can quickly approach the required speed;
Adjust the accumulated value of the speed error with the integral parameter (ki_speed) to eliminate the static error.

For example:

Supposed that there is a water vat, ensure that the water level in the vat is maintained at a height of 1 meter forever.
Suppose the initial water level is 0.2 meter, then there is an error between the current water level and the target water level, and the error is 0.8m.
At this time, you want to control the water level by adding water.
If simply use the Proportional control algorithm, the amount of water added (u) is proportional to the error. That is, u=kp*error

Supposed kp=0.5, t=1 (add water in the first time), so u=0.5*0.8=0.4 , should add the water quantity of 0.4m; now the current water level should be 0.2m+0.4m=0.6m
Then t=2 (add water in the second time), this time water level 0.6m, error 0.4m, so should add the water quantity u=0.5*0.4=0.2 ; now the current water level should be 0.6m+0.2m=0.8m
Followed by this calculation. The water level will reach 1m.
But there are some shortcomings in such single proportional control, one of which is – steady state error!

Like the above example, depending on the value of kp, the system will eventually reach 1 meter without steady state error.
However, considering another situation, suppose that there is water leakage in the process of adding water to the water tank. It is assumed that the water of 0.1 m height will be missed every time the water is added.

Supposed kp=0.5, after several times of adding water, the water level in the water tank will reach 0.8m, the water level will not change again! Because the water level is 0.8m, the error =1-0.8=0.2m.
Therefore, the amount of water added to the water tank is u=0.5*0.2=0.1m.
At the same time, 0.1m of water will flow out from the water tank every time. The added water offsets the leaked water, so the water level will not change!

In other words, the target water level is 1m, but finally the water system reaches 0.8m and not changes. The system has reached stability. The resulting error is the steady-state error.
Therefore, separate proportional control does not meet the requirements in many cases. So we need integral control algorithm.

From the above example, you can find that if only use proportional control, may exist the Transient error, so the water level will be only 0.8m.
In control, we introduce a component that is proportional to the integral of the error.

Therefore, the proportional + integral control algorithm is:
u=kp*error+ ki∗∫error

Here we still use the example mentioned above.
The error of the first time is 0.8, and error of the second time is 0.4. At this point, the integral of the error (in the case of discrete, the integral is actually accumulating), ∫error=0.8+0.4=1.2. The amount of control at this time, in addition to a part of proportion, another part is a coefficient ki multiplied by this integral term.

Since this integral term will accumulate the previous errors, the steady-state error can be well eliminated.
(assuming that in the case of only proportional terms, the system is stuck in the steady-state error, i.e. 0.8 in the above example. Due to the addition of the integral term, input will increase so that the water level of the tank can be greater than 0.8 and gradually reach the target's 1.0.)
This is the function of integral term.

Source Code:

```#include <MsTimer2.h>        //internal timer 2
#include <PinChangeInt.h>    //This library file can make all pins on the REV4 board as external interrupts.
#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC library

MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz;     // Define three-axis acceleration, three-axis gyroscope variables

//TB6612 pins
const int right_R1=8;
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

///////////////////////angle parameters//////////////////////////////
float angle_X; //Calculate the tilt angle variable about the X axis from the acceleration
float angle_Y; //Calculate the tilt angle variable about the Y axis from the acceleration
float angle0 = 1; //Actual measured angle (ideally 0 degrees)
float Gyro_x,Gyro_y,Gyro_z;  //Angular angular velocity by gyroscope calculation
///////////////////////angle parameters//////////////////////////////

///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001;  //Covariance of gyroscope noise
float Q_gyro = 0.003;    //Covariance of gyroscope drift noise
float R_angle = 0.5;    //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; // The value of dt is the filter sampling time.
float K1 = 0.05; //a function containing the Kalman gain is used to calculate the deviation of the optimal estimate
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;    //Gyro drift

float accelz = 0;
float angle;
float angleY_one;
float angle_speed;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////

//////////////////////PID parameters///////////////////////////////
double kp = 34, ki = 0, kd = 0.62;                   //angle loop parameters
double kp_speed = 3.6, ki_speed = 0.080, kd_speed = 0;   // speed loop parameters
double setp0 = 0; //angle balance point
int PD_pwm;  //angle output
float pwm1=0,pwm2=0;

//////////////////Interrupt speed measurement/////////////////////////////
#define PinA_left 5  //external interrupts
#define PinA_right 4   //external interrupts
volatile long count_right = 0;//Used to calculate the pulse value calculated by the Hall encoder (the volatile long type is to ensure the value is valid)
volatile long count_left = 0;
int speedcc = 0;
//////////////////////pulse calculation/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////PI variable parameters//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;

void setup()
{
// set the pins of motor to OUTPUT
pinMode(right_R1,OUTPUT);
pinMode(right_R2,OUTPUT);
pinMode(left_L1,OUTPUT);
pinMode(left_L2,OUTPUT);
pinMode(PWM_R,OUTPUT);
pinMode(PWM_L,OUTPUT);

digitalWrite(right_R1,1);
digitalWrite(right_R2,0);
digitalWrite(left_L1,0);
digitalWrite(left_L2,1);
analogWrite(PWM_R,0);
analogWrite(PWM_L,0);

pinMode(PinA_left, INPUT);  //speed code wheel input
pinMode(PinA_right, INPUT);

// join I2C bus
Wire.begin();                            //join I2C bus sequence
Serial.begin(9600);                       //open the serial monitor to set the baud rate to 9600
delay(1500);
mpu6050.initialize();                       //initialize MPU6050
delay(2);

//5ms; use timer2 to set timer interruption (note：using timer2 will affect the PWM output of pin3 pin11)
MsTimer2::set(5, DSzhongduan);    //5ms ; execute the function DSzhongduan once
MsTimer2::start();    //start interrupt
}

void loop()
{
Serial.println(angle);
delay(100);
//Serial.println(PD_pwm);
//Serial.println(pwm1);
//Serial.println(pwm2);
//Serial.print("pulseright = ");
//Serial.println(pulseright);
//Serial.print("pulseleft = ");
//Serial.println(pulseleft);
//Serial.println(PI_pwm);
//Serial.println(speeds_filter);
//Serial.println (positions);

//External interrupt for calculating wheel speed
attachPinChangeInterrupt(PinA_left, Code_left, CHANGE);          //PinA_left Level change triggers external interrupt; execute subfunction Code_left
attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);       //PinA_right Level change triggers external interrupt; execute subfunction Code_right
}

/////////////////////Hall calculation/////////////////////////
//left speed code wheel count
void Code_left()
{
count_left ++;
}
//Right speed code wheel count
void Code_right()
{
count_right ++;
}
////////////////////pulse calculation///////////////////////
void countpluse()
{
lz = count_left;     //Assign the value counted by the code wheel to lz
rz = count_right;

count_left = 0;     //Clear the code counter count
count_right = 0;

lpluse = lz;
rpluse = rz;

if ((pwm1 < 0) && (pwm2 < 0))                     //judge the moving direction; if backwards（PWM, namely motor voltage is negative）, pulse number is a negative number
{
rpluse = -rpluse;
lpluse = -lpluse;
}
else if ((pwm1 > 0) && (pwm2 > 0))                 // if backwards（PWM, namely motor voltage is positive）, pulse number is a positive number
{
rpluse = rpluse;
lpluse = lpluse;
}
else if ((pwm1 < 0) && (pwm2 > 0))                 //Judge turning direction of the car;  turn left; Right pulse number is a positive number; Left pulse number is a negative number.
{
rpluse = rpluse;
lpluse = -lpluse;
}
else if ((pwm1 > 0) && (pwm2 < 0))               //Judge turning direction of the car;  turn right; Right pulse number is a negative number; Left pulse number is a positive number.
{
rpluse = -rpluse;
lpluse = lpluse;
}

//enter interrupts per 5ms; pulse number superposes
pulseright += rpluse;
pulseleft += lpluse;
}

/////////////////////////////////interrupts////////////////////////////
void DSzhongduan()
{
sei();  //Allow global interrupts
countpluse();        //Pulse superposition subfunction
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data  ax ay az gx gy gz
angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      //get angle and Kalman filtering
PD();         //angle loop PD control
anglePWM();

cc++;
if(cc>=8)     //5*8=40，40ms entering once speed PI algorithm
{
speedpiout();
cc=0;  //Clear
}
}
///////////////////////////////////////////////////////////

/////////////////////////////angle calculation///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
float Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; negative sign is direction processing
Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
Kalman_Filter(Angle, Gyro_x);            //Kalman Filtering
//Rotation angle Z-axis parameter
Gyro_z = -gz / 131;                      //Z-axis angular velocity
//accelz = az / 16.4;

float angleAx = -atan2(ax, az) * (180 / PI); //Calculate the angle with the x-axis
Gyro_y = -gy / 131.00; //Y-axis angular velocity
Yiorderfilter(angleAx, Gyro_y); //first-order filtering
}
////////////////////////////////////////////////////////////////

///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt;          //Prior estimate
angle_err = angle_m - angle;

Pdot[0] = Q_angle - P[0][1] - P[1][0];    //Differential of azimuth error covariance
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] * dt;    //A priori estimation error covariance differential integral
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

//Intermediate variable of matrix multiplication
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
//Denominator
E = R_angle + C_0 * PCt_0;
//gain value
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;  //Intermediate variable of matrix multiplication
t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;    //Posterior estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

q_bias += K_1 * angle_err;    //Posterior estimate
angle_speed = gyro_m - q_bias;   //The differential of the output value gives the optimal angular velocity
angle += K_0 * angle_err; ////Posterior estimation to get the optimal angle
}

/////////////////////first-order filtering/////////////////
void Yiorderfilter(float angle_m, float gyro_m)
{
angleY_one = K1 * angle_m + (1 - K1) * (angleY_one + gyro_m * dt);
}

//////////////////angle PD////////////////////
void PD()
{
PD_pwm = kp * (angle + angle0) + kd * angle_speed; //PD angle loop control
}

//////////////////speed PI////////////////////
void speedpiout()
{
float speeds = (pulseleft + pulseright) * 1.0;      //Vehicle speed  pulse value
pulseright = pulseleft = 0;      //Clear
speeds_filterold *= 0.7;         //first-order complementary filtering
speeds_filter = speeds_filterold + speeds * 0.3;
speeds_filterold = speeds_filter;
positions += speeds_filter;
positions = constrain(positions, -3550,3550);    //Anti-integral saturation
PI_pwm = ki_speed * (setp0 - positions) + kp_speed * (setp0 - speeds_filter);      //speed loop control PI
}
//////////////////speed PI////////////////////

////////////////////////////PWM end value/////////////////////////////
void anglePWM()
{
pwm2=-PD_pwm - PI_pwm ;           //assign the final value of PWM to motor
pwm1=-PD_pwm - PI_pwm ;

if(pwm1>255)             //limit PWM value not greater than 255
{
pwm1=255;
}
if(pwm1<-255)
{
pwm1=-255;
}
if(pwm2>255)
{
pwm2=255;
}
if(pwm2<-255)
{
pwm2=-255;
}

if(angle>80 || angle<-80)      // the inclined angle of balance car is greater than 45°, motor will stop.
{
pwm1=pwm2=0;
}

if(pwm2>=0)         // determine the motor’s steering and speed according to the positive and negative of PWM
{
digitalWrite(left_L1,LOW);
digitalWrite(left_L2,HIGH);
analogWrite(PWM_L,pwm2);
}
else
{
digitalWrite(left_L1,HIGH);
digitalWrite(left_L2,LOW);
analogWrite(PWM_L,-pwm2);
}

if(pwm1>=0)
{
digitalWrite(right_R1,LOW);
digitalWrite(right_R2,HIGH);
analogWrite(PWM_R,pwm1);
}
else
{
digitalWrite(right_R1,HIGH);
digitalWrite(right_R2,LOW);
analogWrite(PWM_R,-pwm1);
}
}
```

Test Result
Installed well the balance car, upload the source code and power on; turn the power switch ON.
The balance car will stand upright on the desktop.
Then open the Arduino IDE, set the baud rate to 9600, the serial monitor will pop up the inclined value of balance car.

Remove the USB cable and balance the car upright on the ground.

If you push it gently by hand, the car will advance in the direction you push, but the tilt angle of the car is opposite to the direction of the push, so the car will return to the balance point without falling.

Buy Self-balancing Car Kit For Arduino Robot