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

PID/PID.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: basic PID algorithm
dudu941014 0:489498e8dae5 8 // Tool Versions: “Keil 5” or “Mbed Complie Online”
dudu941014 0:489498e8dae5 9 // Description: this part is to build the basic PID control theroy
dudu941014 0:489498e8dae5 10 //
dudu941014 0:489498e8dae5 11 //
dudu941014 0:489498e8dae5 12 //////////////////////////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 13
dudu941014 0:489498e8dae5 14 #include "PID.h"
dudu941014 0:489498e8dae5 15
dudu941014 0:489498e8dae5 16 float I_term_parameter;
dudu941014 0:489498e8dae5 17 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 18 //this part is to initialize the PID struct
dudu941014 0:489498e8dae5 19 //system can invoke this function in main loop ot built different PID controller
dudu941014 0:489498e8dae5 20 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 21 void pidInit(PidObject* pid, const float desired, const float kp,const float ki, const float kd, const float iLimit, const float dt)
dudu941014 0:489498e8dae5 22 {
dudu941014 0:489498e8dae5 23 pid->kp = kp;//PID coefficients
dudu941014 0:489498e8dae5 24 pid->ki = ki;//PID coefficients
dudu941014 0:489498e8dae5 25 pid->kd = kd;//PID coefficients
dudu941014 0:489498e8dae5 26 pid->desired = desired;//PID desired, the default parameter is 0
dudu941014 0:489498e8dae5 27 pid->prevError = 0;
dudu941014 0:489498e8dae5 28 pid->error = 0;
dudu941014 0:489498e8dae5 29 pid->integ = 0;
dudu941014 0:489498e8dae5 30 pid->deri = 0;
dudu941014 0:489498e8dae5 31 pid->prevDeri = 0;
dudu941014 0:489498e8dae5 32 pid->outP = 0;//the output of PID coefficients
dudu941014 0:489498e8dae5 33 pid->outI = 0;//the output of PID coefficients
dudu941014 0:489498e8dae5 34 pid->outD = 0;//the output of PID coefficients
dudu941014 0:489498e8dae5 35 pid->iLimit = iLimit;//it is to limit the output of I term
dudu941014 0:489498e8dae5 36 pid->iLimitLow = -iLimit;//it is to limit the output of I term
dudu941014 0:489498e8dae5 37 pid->dt = dt;//shampling period
dudu941014 0:489498e8dae5 38 }
dudu941014 0:489498e8dae5 39
dudu941014 0:489498e8dae5 40 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 41 //this part is a independent function which calculates the P_term's output
dudu941014 0:489498e8dae5 42 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 43 float get_p(PidObject* pid)
dudu941014 0:489498e8dae5 44 {
dudu941014 0:489498e8dae5 45 pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy
dudu941014 0:489498e8dae5 46 return pid->outP;
dudu941014 0:489498e8dae5 47 }
dudu941014 0:489498e8dae5 48
dudu941014 0:489498e8dae5 49 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 50 //this part is a independent function which calculates the I_term's output, but this part has been changed to adapt veloctiy PID controller
dudu941014 0:489498e8dae5 51 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 52 float get_i(PidObject* pid, float distance)
dudu941014 0:489498e8dae5 53 {
dudu941014 0:489498e8dae5 54 pid->outI = -pid->ki * distance * 10; //calculate the output of I_term according to PID control theroy
dudu941014 0:489498e8dae5 55 if(pid->outI > pid->iLimit)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 56 {
dudu941014 0:489498e8dae5 57 pid->outI = pid->iLimit;
dudu941014 0:489498e8dae5 58 }
dudu941014 0:489498e8dae5 59 else if(pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 60 {
dudu941014 0:489498e8dae5 61 pid->outI = pid->iLimitLow;
dudu941014 0:489498e8dae5 62 }
dudu941014 0:489498e8dae5 63 return pid->outI;
dudu941014 0:489498e8dae5 64 }
dudu941014 0:489498e8dae5 65
dudu941014 0:489498e8dae5 66 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 67 //this part is a independent function which calculates the D_term's output
dudu941014 0:489498e8dae5 68 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 69 float get_d(PidObject* pid)
dudu941014 0:489498e8dae5 70 {
dudu941014 0:489498e8dae5 71 pid->deri = (pid->error - pid->prevError) / pid->dt; //calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 72 pid->deri = pid->prevDeri + ( pid->dt / ( PID_D_TERM_FILTER + pid->dt ) ) * ( pid->deri - pid->prevDeri );//this is a filter
dudu941014 0:489498e8dae5 73
dudu941014 0:489498e8dae5 74 pid->prevDeri = pid->deri;
dudu941014 0:489498e8dae5 75 pid->prevError = pid->error;
dudu941014 0:489498e8dae5 76
dudu941014 0:489498e8dae5 77 pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 78
dudu941014 0:489498e8dae5 79 return pid->outD;
dudu941014 0:489498e8dae5 80 }
dudu941014 0:489498e8dae5 81
dudu941014 0:489498e8dae5 82 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 83 //this part is a function which calculates the P_term's I_term's and D_term's output together
dudu941014 0:489498e8dae5 84 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 85 float show_I_term(void)// in order to oberse the change of I term
dudu941014 0:489498e8dae5 86 {
dudu941014 0:489498e8dae5 87 return I_term_parameter;
dudu941014 0:489498e8dae5 88 }
dudu941014 0:489498e8dae5 89
dudu941014 0:489498e8dae5 90
dudu941014 0:489498e8dae5 91 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 92 //this part is basic the PID controller for Velocity PID
dudu941014 0:489498e8dae5 93 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 94 float get_pid_vel(PidObject* pid, float distance)
dudu941014 0:489498e8dae5 95 {
dudu941014 0:489498e8dae5 96 float output;
dudu941014 0:489498e8dae5 97
dudu941014 1:ce626b794a6c 98 pid->outP = pid->kp * pid->error; //calculate the output of P_term according to PID control theroy
dudu941014 0:489498e8dae5 99
dudu941014 0:489498e8dae5 100 //pid->outI += pid->ki * pid->error * pid->dt; //I
dudu941014 1:ce626b794a6c 101 pid->outI = -pid->ki * distance * 10;//calculate the output of I_term according to PID control theroy
dudu941014 1:ce626b794a6c 102 //this code is the difference between Velocity PID and Angle PID
dudu941014 0:489498e8dae5 103
dudu941014 1:ce626b794a6c 104 if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 105 {
dudu941014 0:489498e8dae5 106 pid->outI = pid->iLimit;
dudu941014 0:489498e8dae5 107 }
dudu941014 1:ce626b794a6c 108 else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 109 {
dudu941014 0:489498e8dae5 110 pid->outI = pid->iLimitLow;
dudu941014 0:489498e8dae5 111 }
dudu941014 0:489498e8dae5 112
dudu941014 0:489498e8dae5 113 I_term_parameter = pid->outI;
dudu941014 0:489498e8dae5 114
dudu941014 1:ce626b794a6c 115 pid->deri = (pid->error - pid->prevError) / pid->dt; //calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 116 pid->prevError = pid->error;
dudu941014 0:489498e8dae5 117
dudu941014 1:ce626b794a6c 118 pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 119
dudu941014 1:ce626b794a6c 120 output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT
dudu941014 0:489498e8dae5 121
dudu941014 1:ce626b794a6c 122 return output;//returen the total OUTPUT
dudu941014 0:489498e8dae5 123 }
dudu941014 0:489498e8dae5 124
dudu941014 0:489498e8dae5 125
dudu941014 0:489498e8dae5 126 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 127 //this part is basic the PID controller for Angle PID
dudu941014 0:489498e8dae5 128 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 129 float get_pid(PidObject* pid)
dudu941014 0:489498e8dae5 130 {
dudu941014 0:489498e8dae5 131 float output;
dudu941014 0:489498e8dae5 132
dudu941014 0:489498e8dae5 133 pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy
dudu941014 0:489498e8dae5 134
dudu941014 0:489498e8dae5 135 pid->outI += pid->ki * pid->error * pid->dt;//calculate the output of I_term according to PID control theroy
dudu941014 0:489498e8dae5 136
dudu941014 0:489498e8dae5 137
dudu941014 0:489498e8dae5 138 if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 139 {
dudu941014 0:489498e8dae5 140 pid->outI = pid->iLimit;
dudu941014 0:489498e8dae5 141 }
dudu941014 0:489498e8dae5 142 else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 143 {
dudu941014 0:489498e8dae5 144 pid->outI = pid->iLimitLow;
dudu941014 0:489498e8dae5 145 }//to limit the output of the PID controller
dudu941014 0:489498e8dae5 146
dudu941014 0:489498e8dae5 147 I_term_parameter = pid->outI;// in order to oberse the change of I term
dudu941014 0:489498e8dae5 148
dudu941014 0:489498e8dae5 149 pid->deri = (pid->error - pid->prevError) / pid->dt;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 150 pid->prevError = pid->error;
dudu941014 0:489498e8dae5 151
dudu941014 0:489498e8dae5 152 pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 153
dudu941014 0:489498e8dae5 154 output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT
dudu941014 0:489498e8dae5 155
dudu941014 0:489498e8dae5 156 return output;//returen the total OUTPUT
dudu941014 0:489498e8dae5 157 }
dudu941014 0:489498e8dae5 158 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 159 //this part is to reset the PID struct
dudu941014 0:489498e8dae5 160 //if the I_term is so lager, this function can be used to reset the I_term!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
dudu941014 0:489498e8dae5 161 //otherwise, the balancer would be unstable
dudu941014 0:489498e8dae5 162 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 163 void pidReset(PidObject* pid)
dudu941014 0:489498e8dae5 164 {
dudu941014 0:489498e8dae5 165 pid->error = 0;//reset the error to 0
dudu941014 0:489498e8dae5 166 pid->prevError = 0;//reset the preverror to 0
dudu941014 0:489498e8dae5 167 pid->integ = 0;//reset the integ to 0
dudu941014 0:489498e8dae5 168 pid->deri = 0;//reset the deri to 0
dudu941014 0:489498e8dae5 169 }
dudu941014 0:489498e8dae5 170
dudu941014 0:489498e8dae5 171 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 172 //this part is to set the PID error manually
dudu941014 0:489498e8dae5 173 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 174 void pidSetError(PidObject* pid, const float error)
dudu941014 0:489498e8dae5 175 {
dudu941014 0:489498e8dae5 176 pid->error = error;//this part is to set the PID error manually
dudu941014 0:489498e8dae5 177 }
dudu941014 0:489498e8dae5 178 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 179 //this part is to set the PID desire manually
dudu941014 0:489498e8dae5 180 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 181 void pidSetDesired(PidObject* pid, const float desired)
dudu941014 0:489498e8dae5 182 {
dudu941014 0:489498e8dae5 183 pid->desired = desired;//this part is to set the PID error manually
dudu941014 0:489498e8dae5 184 }