Diff: PID/PID.cpp

Revision:

0:489498e8dae5

Child:

1:ce626b794a6c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp	Fri Aug 25 21:10:23 2017 +0000
@@ -0,0 +1,183 @@
+//////////////////////////////////////////////////////////////////////////////////
+// Company: edinburgh of university
+// Engineer: ZEjun DU
+// 
+// Create Date: 2017/08/20 13:06:52
+// Design Name: Inverted Pendulum Balancer
+// Module Name: basic PID algorithm
+// Tool Versions: “Keil 5” or “Mbed Complie Online”
+// Description: this part is to build the basic PID control theroy
+//                           
+// 
+//////////////////////////////////////////////////////////////////////////////////
+
+#include "PID.h"
+
+float I_term_parameter;
+////////////////////////////////////////////////////////////
+//this part is to initialize the PID struct 
+//system can invoke this function in main loop ot built different PID controller
+////////////////////////////////////////////////////////////
+void pidInit(PidObject* pid, const float desired, const float kp,const float ki, const float kd, const float iLimit, const float dt)
+{
+    pid->kp = kp;//PID coefficients
+    pid->ki = ki;//PID coefficients
+    pid->kd = kd;//PID coefficients     
+    pid->desired = desired;//PID desired, the default parameter is 0
+    pid->prevError = 0;
+    pid->error = 0;
+    pid->integ = 0;
+    pid->deri = 0;
+    pid->prevDeri = 0;
+    pid->outP = 0;//the output of PID coefficients
+    pid->outI = 0;//the output of PID coefficients
+    pid->outD = 0;//the output of PID coefficients
+    pid->iLimit = iLimit;//it is to limit the output of I term
+    pid->iLimitLow = -iLimit;//it is to limit the output of I term
+    pid->dt = dt;//shampling period 
+}
+
+////////////////////////////////////////////////////////////
+//this part is a independent function which calculates the P_term's output
+////////////////////////////////////////////////////////////
+float get_p(PidObject* pid)
+{
+    pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy 
+    return pid->outP;
+}
+
+////////////////////////////////////////////////////////////
+//this part is a independent function which calculates the I_term's output, but this part has been changed to adapt veloctiy PID controller
+////////////////////////////////////////////////////////////
+float get_i(PidObject* pid, float distance)
+{
+    pid->outI = -pid->ki * distance * 10; //calculate the output of I_term according to PID control theroy 
+    if(pid->outI > pid->iLimit)//this part is to limit the output of I_term
+    {
+        pid->outI = pid->iLimit;
+    }
+    else if(pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
+    {
+        pid->outI = pid->iLimitLow;
+    }
+    return pid->outI;
+}
+
+////////////////////////////////////////////////////////////
+//this part is a independent function which calculates the D_term's output
+////////////////////////////////////////////////////////////
+float get_d(PidObject* pid)
+{
+    pid->deri = (pid->error - pid->prevError) / pid->dt;    //calculate the output of D_term according to PID control theroy 
+    pid->deri = pid->prevDeri + ( pid->dt / ( PID_D_TERM_FILTER + pid->dt ) ) * ( pid->deri - pid->prevDeri );//this is a filter
+    
+    pid->prevDeri = pid->deri;
+    pid->prevError = pid->error;
+    
+    pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy 
+    
+    return pid->outD;
+}
+
+////////////////////////////////////////////////////////////
+//this part is a function which calculates the P_term's I_term's and D_term's output together
+////////////////////////////////////////////////////////////
+float show_I_term(void)// in order to oberse the change of I term
+    {
+        return I_term_parameter;
+    }
+
+
+////////////////////////////////////////////////////////////
+//this part is basic the PID controller for Velocity PID
+////////////////////////////////////////////////////////////
+float get_pid_vel(PidObject* pid, float distance)
+{
+    float output;
+    
+    pid->outP = pid->kp * pid->error;   //P
+    
+    //pid->outI += pid->ki * pid->error * pid->dt;    //I
+    pid->outI = -pid->ki * distance * 10;
+
+    if (pid->outI > pid->iLimit)
+    {
+        pid->outI = pid->iLimit;
+    }   
+    else if (pid->outI < pid->iLimitLow)
+    {
+        pid->outI = pid->iLimitLow;
+    }
+    
+    I_term_parameter = pid->outI;
+    
+    pid->deri = (pid->error - pid->prevError) / pid->dt;    //D
+    pid->prevError = pid->error;
+    
+    pid->outD = pid->kd * pid->deri;
+    
+    output = pid->outP + pid->outI + pid->outD;
+    
+    return output;
+}
+
+
+////////////////////////////////////////////////////////////
+//this part is basic the PID controller for Angle PID
+////////////////////////////////////////////////////////////
+float get_pid(PidObject* pid)
+{
+    float output;
+    
+    pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy 
+    
+    pid->outI += pid->ki * pid->error * pid->dt;//calculate the output of I_term according to PID control theroy 
+    
+
+    if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
+    {
+        pid->outI = pid->iLimit;
+    }   
+    else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
+    {
+        pid->outI = pid->iLimitLow;
+    }//to limit the output of the PID controller
+    
+    I_term_parameter = pid->outI;// in order to oberse the change of I term
+    
+    pid->deri = (pid->error - pid->prevError) / pid->dt;//calculate the output of D_term according to PID control theroy 
+    pid->prevError = pid->error;
+    
+    pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy 
+    
+    output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT 
+    
+    return output;//returen the total OUTPUT 
+}
+////////////////////////////////////////////////////////////
+//this part is to reset the PID struct 
+//if the I_term is so lager, this function can be used to reset the I_term!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//otherwise, the balancer would be unstable
+////////////////////////////////////////////////////////////
+void pidReset(PidObject* pid)
+{
+  pid->error = 0;//reset the error to 0
+  pid->prevError = 0;//reset the preverror to 0
+  pid->integ = 0;//reset the integ to 0
+  pid->deri = 0;//reset the deri to 0
+}
+
+////////////////////////////////////////////////////////////
+//this part is to set the PID error manually  
+////////////////////////////////////////////////////////////
+void pidSetError(PidObject* pid, const float error)
+{
+  pid->error = error;//this part is to set the PID error manually  
+}
+////////////////////////////////////////////////////////////
+//this part is to set the PID desire manually  
+////////////////////////////////////////////////////////////
+void pidSetDesired(PidObject* pid, const float desired)
+{
+  pid->desired = desired;//this part is to set the PID error manually  
+}
\ No newline at end of file