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@0:489498e8dae5, 2017-08-25 (annotated)

Committer:
dudu941014
Date:
Fri Aug 25 21:10:23 2017 +0000
Revision:
0:489498e8dae5
Child:
1:ce626b794a6c
this code is for inverted pendulum balancer. it is based on two-loop PID controller. one PID 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 0:489498e8dae5 98 pid->outP = pid->kp * pid->error; //P
dudu941014 0:489498e8dae5 99
dudu941014 0:489498e8dae5 100 //pid->outI += pid->ki * pid->error * pid->dt; //I
dudu941014 0:489498e8dae5 101 pid->outI = -pid->ki * distance * 10;
dudu941014 0:489498e8dae5 102
dudu941014 0:489498e8dae5 103 if (pid->outI > pid->iLimit)
dudu941014 0:489498e8dae5 104 {
dudu941014 0:489498e8dae5 105 pid->outI = pid->iLimit;
dudu941014 0:489498e8dae5 106 }
dudu941014 0:489498e8dae5 107 else if (pid->outI < pid->iLimitLow)
dudu941014 0:489498e8dae5 108 {
dudu941014 0:489498e8dae5 109 pid->outI = pid->iLimitLow;
dudu941014 0:489498e8dae5 110 }
dudu941014 0:489498e8dae5 111
dudu941014 0:489498e8dae5 112 I_term_parameter = pid->outI;
dudu941014 0:489498e8dae5 113
dudu941014 0:489498e8dae5 114 pid->deri = (pid->error - pid->prevError) / pid->dt; //D
dudu941014 0:489498e8dae5 115 pid->prevError = pid->error;
dudu941014 0:489498e8dae5 116
dudu941014 0:489498e8dae5 117 pid->outD = pid->kd * pid->deri;
dudu941014 0:489498e8dae5 118
dudu941014 0:489498e8dae5 119 output = pid->outP + pid->outI + pid->outD;
dudu941014 0:489498e8dae5 120
dudu941014 0:489498e8dae5 121 return output;
dudu941014 0:489498e8dae5 122 }
dudu941014 0:489498e8dae5 123
dudu941014 0:489498e8dae5 124
dudu941014 0:489498e8dae5 125 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 126 //this part is basic the PID controller for Angle PID
dudu941014 0:489498e8dae5 127 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 128 float get_pid(PidObject* pid)
dudu941014 0:489498e8dae5 129 {
dudu941014 0:489498e8dae5 130 float output;
dudu941014 0:489498e8dae5 131
dudu941014 0:489498e8dae5 132 pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy
dudu941014 0:489498e8dae5 133
dudu941014 0:489498e8dae5 134 pid->outI += pid->ki * pid->error * pid->dt;//calculate the output of I_term according to PID control theroy
dudu941014 0:489498e8dae5 135
dudu941014 0:489498e8dae5 136
dudu941014 0:489498e8dae5 137 if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 138 {
dudu941014 0:489498e8dae5 139 pid->outI = pid->iLimit;
dudu941014 0:489498e8dae5 140 }
dudu941014 0:489498e8dae5 141 else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
dudu941014 0:489498e8dae5 142 {
dudu941014 0:489498e8dae5 143 pid->outI = pid->iLimitLow;
dudu941014 0:489498e8dae5 144 }//to limit the output of the PID controller
dudu941014 0:489498e8dae5 145
dudu941014 0:489498e8dae5 146 I_term_parameter = pid->outI;// in order to oberse the change of I term
dudu941014 0:489498e8dae5 147
dudu941014 0:489498e8dae5 148 pid->deri = (pid->error - pid->prevError) / pid->dt;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 149 pid->prevError = pid->error;
dudu941014 0:489498e8dae5 150
dudu941014 0:489498e8dae5 151 pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy
dudu941014 0:489498e8dae5 152
dudu941014 0:489498e8dae5 153 output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT
dudu941014 0:489498e8dae5 154
dudu941014 0:489498e8dae5 155 return output;//returen the total OUTPUT
dudu941014 0:489498e8dae5 156 }
dudu941014 0:489498e8dae5 157 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 158 //this part is to reset the PID struct
dudu941014 0:489498e8dae5 159 //if the I_term is so lager, this function can be used to reset the I_term!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
dudu941014 0:489498e8dae5 160 //otherwise, the balancer would be unstable
dudu941014 0:489498e8dae5 161 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 162 void pidReset(PidObject* pid)
dudu941014 0:489498e8dae5 163 {
dudu941014 0:489498e8dae5 164 pid->error = 0;//reset the error to 0
dudu941014 0:489498e8dae5 165 pid->prevError = 0;//reset the preverror to 0
dudu941014 0:489498e8dae5 166 pid->integ = 0;//reset the integ to 0
dudu941014 0:489498e8dae5 167 pid->deri = 0;//reset the deri to 0
dudu941014 0:489498e8dae5 168 }
dudu941014 0:489498e8dae5 169
dudu941014 0:489498e8dae5 170 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 171 //this part is to set the PID error manually
dudu941014 0:489498e8dae5 172 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 173 void pidSetError(PidObject* pid, const float error)
dudu941014 0:489498e8dae5 174 {
dudu941014 0:489498e8dae5 175 pid->error = error;//this part is to set the PID error manually
dudu941014 0:489498e8dae5 176 }
dudu941014 0:489498e8dae5 177 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 178 //this part is to set the PID desire manually
dudu941014 0:489498e8dae5 179 ////////////////////////////////////////////////////////////
dudu941014 0:489498e8dae5 180 void pidSetDesired(PidObject* pid, const float desired)
dudu941014 0:489498e8dae5 181 {
dudu941014 0:489498e8dae5 182 pid->desired = desired;//this part is to set the PID error manually
dudu941014 0:489498e8dae5 183 }