zejun du / Mbed 2 deprecated inverted_pendulum_system

this code is for inverted pendulum balancer. it is based on two-loop PID controller. One controller is for Angle and another one is for velocity.

Dependencies:   HCSR04 L298HBridge mbed

controller/controller.cpp@1:ce626b794a6c, 2017-08-25 (annotated)

Committer:
dudu941014
Date:
Fri Aug 25 21:25:46 2017 +0000
Revision:
1:ce626b794a6c
Parent:
0:489498e8dae5 
this code is for inverted pendulum balancer. it is based on two-loop PID controller. One controller is for Angle and another one is for velocity.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dudu941014 0:489498e8dae5 1 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 2 // Company: edinburgh of university
dudu941014 0:489498e8dae5 3 // Engineer: ZEjun DU
dudu941014 0:489498e8dae5 4 //
dudu941014 0:489498e8dae5 5 // Create Date: 2017/08/20 13:06:52
dudu941014 0:489498e8dae5 6 // Design Name: Inverted Pendulum Balancer
dudu941014 0:489498e8dae5 7 // Module Name: create the PID struct, and control and manage two PID controllers
dudu941014 0:489498e8dae5 8 // Tool Versions: “Keil 5” or “Mbed Complie Online”
dudu941014 0:489498e8dae5 9 // Description: this part is to get PWM result from two PID controllers
dudu941014 0:489498e8dae5 10 //
dudu941014 0:489498e8dae5 11 //
dudu941014 0:489498e8dae5 12 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 13
dudu941014 0:489498e8dae5 14 #include "controller.h"
dudu941014 0:489498e8dae5 15
dudu941014 0:489498e8dae5 16 PidObject pidAngle;//create Angle PID controller
dudu941014 0:489498e8dae5 17 PidObject pidVelocity;//create Velocity PID controller
dudu941014 0:489498e8dae5 18
dudu941014 0:489498e8dae5 19 static float LastEncoder,Encoder,LastDistance_result;
dudu941014 0:489498e8dae5 20
dudu941014 0:489498e8dae5 21 float AngleOutput;//the angle PID controller's output
dudu941014 0:489498e8dae5 22 float velOutput;//the Velocity PID controller's output
dudu941014 0:489498e8dae5 23 L298HBridge Motor_PWM(p21, p22, p23);//it defines the pins of controlling motor
dudu941014 0:489498e8dae5 24
dudu941014 0:489498e8dae5 25 float Motor;//the PWM result from two PID controller
dudu941014 0:489498e8dae5 26
dudu941014 0:489498e8dae5 27 float I_term;//show the changle of I_term
dudu941014 0:489498e8dae5 28
dudu941014 0:489498e8dae5 29
dudu941014 0:489498e8dae5 30 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 31 //this funcition is to initialize two PID controllers
dudu941014 0:489498e8dae5 32 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 33 void controller_init(void)
dudu941014 0:489498e8dae5 34 {
dudu941014 0:489498e8dae5 35 pidInit(&pidAngle, 0, 0, 0, 0, 50, controllerAng_update_dt);//to create and initialize angle PID controller
dudu941014 0:489498e8dae5 36 pidInit(&pidVelocity, 0, 0, 0, 0, 500, controllerVel_Update_dt);//to create and initialize Velocity PID controller
dudu941014 0:489498e8dae5 37 }
dudu941014 0:489498e8dae5 38
dudu941014 0:489498e8dae5 39
dudu941014 0:489498e8dae5 40 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 41 //this funcition is to control and manage the Angle PID controller
dudu941014 0:489498e8dae5 42 //the system can invoke this function to get the result of Angle PWM
dudu941014 0:489498e8dae5 43 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 44 float controller_Angle_PID(float angle_result)
dudu941014 0:489498e8dae5 45 {
dudu941014 0:489498e8dae5 46 // Update PID for angle
dudu941014 0:489498e8dae5 47 pidAngle.error = pidAngle.desired - angle_result * 10;// in the actual control, the effect of plusing 0.01 for Kp Ki Kd is too small. thus the error is multiplied 10 rather than change GUI's code
dudu941014 0:489498e8dae5 48 if(pidAngle.error > AngleErrLimit)//this part is to limit the error of angle
dudu941014 0:489498e8dae5 49 pidAngle.error = AngleErrLimit;//if the angle's error is too lager, the result of angle will become limitation
dudu941014 0:489498e8dae5 50 else if(pidAngle.error < -AngleErrLimit)//this part is to limit the error of angle
dudu941014 0:489498e8dae5 51 pidAngle.error = -AngleErrLimit;//if the angle's error is too small, the result of angle will become limitation
dudu941014 0:489498e8dae5 52 AngleOutput = get_pid(&pidAngle);//get the angle output from basic PID control theroy
dudu941014 0:489498e8dae5 53
dudu941014 0:489498e8dae5 54 if(AngleOutput > AngleOutputLimit)//judge the angle output
dudu941014 0:489498e8dae5 55 AngleOutput = AngleOutputLimit;//if the angle's output is too lager, the output of the angle PID controller will become limitation
dudu941014 0:489498e8dae5 56 else if(AngleOutput < -AngleOutputLimit)//judge the angle output
dudu941014 0:489498e8dae5 57 AngleOutput = -AngleOutputLimit;//if the angle's output is too small, the output of the angle PID controller will become limitation
dudu941014 0:489498e8dae5 58
dudu941014 0:489498e8dae5 59 return AngleOutput;//return the output of the angle PID controller
dudu941014 0:489498e8dae5 60
dudu941014 0:489498e8dae5 61 }
dudu941014 0:489498e8dae5 62
dudu941014 0:489498e8dae5 63
dudu941014 0:489498e8dae5 64 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 65 //this funcition is to control and manage the Velocity PID controller
dudu941014 0:489498e8dae5 66 //the system can invoke this function to get the result of Velocity PWM
dudu941014 0:489498e8dae5 67 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 68 float controller_Velocity_PID(float Distance_result)
dudu941014 0:489498e8dae5 69 {
dudu941014 0:489498e8dae5 70 LastEncoder =(Distance_result- LastDistance_result) / controllerVel_Update_dt; //to calculate the velocity
dudu941014 0:489498e8dae5 71 Encoder *= 0.8; //this equation can be ragarded as a low pass filter, it is used to reduce the effect of Velocity PID controller
dudu941014 0:489498e8dae5 72 Encoder += LastEncoder*0.2; //this equation can be replaced by the "mean function", for example, (Encoder(1)+Encoder(2)+Encoder(3))/3
dudu941014 0:489498e8dae5 73
dudu941014 0:489498e8dae5 74 pidVelocity.error = pidVelocity.desired - Encoder * 10;// in the actual control, the effect of plusing 0.01 for Kp Ki Kd is too small. thus the error is multiplied 10 rather than change GUI's code
dudu941014 0:489498e8dae5 75
dudu941014 0:489498e8dae5 76 if(pidVelocity.error > VelocityLimit)//this part is to limit the error of Velocity
dudu941014 0:489498e8dae5 77 pidVelocity.error = VelocityLimit;//if the Velocity's error is too lager, the result of Velocity will become limitation
dudu941014 0:489498e8dae5 78 else if(pidVelocity.error < -VelocityLimit)//this part is to limit the error of Velocity
dudu941014 0:489498e8dae5 79 pidVelocity.error = -VelocityLimit;//if the Velocity's error is too lager, the result of Velocity will become limitation
dudu941014 0:489498e8dae5 80 velOutput= get_pid_vel(&pidVelocity,Distance_result);//in order to set intput of I_term, the output of velocity become this
dudu941014 0:489498e8dae5 81 I_term = show_I_term();//show the change of I_term on GUI
dudu941014 0:489498e8dae5 82
dudu941014 0:489498e8dae5 83 if(velOutput > velOutputLimit)//judge the velocity output
dudu941014 0:489498e8dae5 84 velOutput = velOutputLimit;//if the velocity's output is too lager, the output of the veloctiy PID controller will become limitation
dudu941014 0:489498e8dae5 85 else if(velOutput < -velOutputLimit)//judge the velocity output
dudu941014 0:489498e8dae5 86 velOutput = -velOutputLimit;//if the velocity's output is too small, the output of the velocity PID controller will become limitation
dudu941014 0:489498e8dae5 87
dudu941014 0:489498e8dae5 88
dudu941014 0:489498e8dae5 89 LastDistance_result = Distance_result;//record the last velocity
dudu941014 0:489498e8dae5 90
dudu941014 0:489498e8dae5 91 return velOutput;//return the result of the velocity PID controller
dudu941014 0:489498e8dae5 92 }
dudu941014 0:489498e8dae5 93
dudu941014 0:489498e8dae5 94
dudu941014 0:489498e8dae5 95 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 96 //this funcition is to reset parameters of two PID controllers
dudu941014 0:489498e8dae5 97 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 98 void controllerResetAllPID(void)
dudu941014 0:489498e8dae5 99 {
dudu941014 0:489498e8dae5 100 pidReset(&pidAngle);//reset angle PID controller
dudu941014 0:489498e8dae5 101 pidReset(&pidVelocity);//reset velocity PID controller
dudu941014 0:489498e8dae5 102 }
dudu941014 0:489498e8dae5 103
dudu941014 0:489498e8dae5 104
dudu941014 0:489498e8dae5 105 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 106 //this funcition is to control the motor according to current PID controller
dudu941014 0:489498e8dae5 107 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 108 void MOTOR_Update(float Motor)
dudu941014 0:489498e8dae5 109 {
dudu941014 0:489498e8dae5 110 if(Motor >= 1000)//this judgement limits the output of motor
dudu941014 0:489498e8dae5 111 {
dudu941014 0:489498e8dae5 112 Motor = 1000;
dudu941014 0:489498e8dae5 113 }
dudu941014 0:489498e8dae5 114 else if(Motor <= -1000)//this judgement limits the output of motor
dudu941014 0:489498e8dae5 115 {
dudu941014 0:489498e8dae5 116 Motor = -1000;
dudu941014 0:489498e8dae5 117 }
dudu941014 0:489498e8dae5 118 if(Motor > 0)//this judgement can give the direction of current motor value. go forward
dudu941014 0:489498e8dae5 119 {
dudu941014 0:489498e8dae5 120 Motor_PWM.Fwd();//go forward
dudu941014 0:489498e8dae5 121 Motor_PWM.Speed((float)Motor + offset_moving);//set the force to motor according to PID controllers
dudu941014 0:489498e8dae5 122 }
dudu941014 0:489498e8dae5 123 //if the balancer is under the load, the car cannot get enough force to make the movement at 10% PWM or 20% PWM
dudu941014 0:489498e8dae5 124 // after many times tests, the offset PWM is 40%
dudu941014 0:489498e8dae5 125
dudu941014 0:489498e8dae5 126 if(Motor == 0)//if the output is 0, the motor will be commended to stop
dudu941014 0:489498e8dae5 127 {
dudu941014 0:489498e8dae5 128 Motor_PWM.Stop();
dudu941014 0:489498e8dae5 129 Motor_PWM.Speed(Motor);
dudu941014 0:489498e8dae5 130 }
dudu941014 0:489498e8dae5 131 if(Motor < 0)//this judgement can give the direction of current motor value. go reserve
dudu941014 0:489498e8dae5 132 {
dudu941014 0:489498e8dae5 133 Motor_PWM.Rev();//go reserve
dudu941014 0:489498e8dae5 134 Motor_PWM.Speed(-(float)Motor + offset_moving);//set the force to motor according to PID controllers
dudu941014 0:489498e8dae5 135 }
dudu941014 0:489498e8dae5 136 }