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); //assign initial state value 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.

- Self-balancing Car Kit For Arduino Robot Tutorial
- Self-balancing Car Kit For Arduino Robot Project 1: Getting Started with Main Board and ARDUINO
- Self-balancing Car Kit For Arduino Robot Project 2: Button and Buzzer
- Self-balancing Car Kit For Arduino Robot Project 3: TB6612 Motor Driving
- Self-balancing Car Kit For Arduino Robot Project 4: Hall Encoder Test
- Self-balancing Car Kit For Arduino Robot Project 5: Internal Timer Interrupt
- Self-balancing Car Kit For Arduino Robot Project 6: Bluetooth Test
- Self-balancing Car Kit For Arduino Robot Project 7: MPU6050 Test
- Self-balancing Car Kit For Arduino Robot Project 8: Calculating Inclined Angle And Angular Velocity Values
- Self-balancing Car Kit For Arduino Robot Project 9: PID Principle
- Self-balancing Car Kit For Arduino Robot Project 10: Upright Loop Adjustment
- Self-balancing Car Kit For Arduino Robot Project 11: Speed Loop Adjustment
- Self-balancing Car Kit For Arduino Robot Project 12: Steering Loop Control
- Self-balancing Car Kit For Arduino Robot Project 13: Bluetooth Control
- Self-balancing Car Kit For Arduino Robot Project 14: Adjusting Balance Angle and Bluetooth Control