Chun Feng Huang / PID

New PID library with digital anti-windup and process control

Fork of PID_modified by Chun Feng Huang

PID.cpp@2:b9610a2d2ea0, 2016-10-25 (annotated)

Committer:
benson516
Date:
Tue Oct 25 13:04:58 2016 +0000
Revision:
2:b9610a2d2ea0
Parent:
1:4df4895863cd
Child:
3:d8646d8c994f
First run succeed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
weisnail 0:7f9b4ca968ae 1 #include "PID.h"
weisnail 0:7f9b4ca968ae 2
benson516 2:b9610a2d2ea0 3 PID::PID(float Kp_in, float Ki_in, float Kd_in, float Sampletime_in){
weisnail 0:7f9b4ca968ae 4
benson516 2:b9610a2d2ea0 5 // Parameters
weisnail 0:7f9b4ca968ae 6 Outputlimit_bool = false;
weisnail 0:7f9b4ca968ae 7 Inputlimit_bool = false;
adam_z 1:4df4895863cd 8 AntiWindUp_bool = false;
weisnail 0:7f9b4ca968ae 9 outputLimits_H = 0.0;
weisnail 0:7f9b4ca968ae 10 outputLimits_L = 0.0;
weisnail 0:7f9b4ca968ae 11 inputLimits_H = 0.0;
weisnail 0:7f9b4ca968ae 12 inputLimits_L = 0.0;
benson516 2:b9610a2d2ea0 13 feedbackvalue = 0.0;
benson516 2:b9610a2d2ea0 14
benson516 2:b9610a2d2ea0 15 // Feedback gain
benson516 2:b9610a2d2ea0 16 Kp = Kp_in;
benson516 2:b9610a2d2ea0 17 Ki = Ki_in;
benson516 2:b9610a2d2ea0 18 Kd = Kd_in;
benson516 2:b9610a2d2ea0 19
benson516 2:b9610a2d2ea0 20 // Sampling time
benson516 2:b9610a2d2ea0 21 Ts = Sampletime_in;
weisnail 0:7f9b4ca968ae 22
benson516 2:b9610a2d2ea0 23 // Variables
benson516 2:b9610a2d2ea0 24 error[0] = 0.0;
benson516 2:b9610a2d2ea0 25 error[1] = 0.0;
benson516 2:b9610a2d2ea0 26 error_I = 0.0;
benson516 2:b9610a2d2ea0 27 //
benson516 2:b9610a2d2ea0 28 reference = 0.0;
benson516 2:b9610a2d2ea0 29 output = 0.0;
benson516 2:b9610a2d2ea0 30
benson516 2:b9610a2d2ea0 31
benson516 2:b9610a2d2ea0 32
weisnail 0:7f9b4ca968ae 33 }
weisnail 0:7f9b4ca968ae 34
weisnail 0:7f9b4ca968ae 35 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
weisnail 0:7f9b4ca968ae 36 Outputlimit_bool = true;
weisnail 0:7f9b4ca968ae 37 outputLimits_H = setoutputLimits_H;
weisnail 0:7f9b4ca968ae 38 outputLimits_L = setoutputLimits_L;
weisnail 0:7f9b4ca968ae 39 }
weisnail 0:7f9b4ca968ae 40
weisnail 0:7f9b4ca968ae 41 void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
weisnail 0:7f9b4ca968ae 42 Inputlimit_bool = true;
weisnail 0:7f9b4ca968ae 43 inputLimits_H = setinputLimits_H;
weisnail 0:7f9b4ca968ae 44 inputLimits_L = setinputLimits_L;
weisnail 0:7f9b4ca968ae 45 }
weisnail 0:7f9b4ca968ae 46
adam_z 1:4df4895863cd 47 void PID::EnableAntiWindUp(float Ka_)
adam_z 1:4df4895863cd 48 {
adam_z 1:4df4895863cd 49 AntiWindUp_bool = true;
adam_z 1:4df4895863cd 50 Ka = Ka_;
adam_z 1:4df4895863cd 51 }
adam_z 1:4df4895863cd 52
benson516 2:b9610a2d2ea0 53 void PID::Compute(float reference_in, float feedbackvalue_in){
weisnail 0:7f9b4ca968ae 54
weisnail 0:7f9b4ca968ae 55 if(Inputlimit_bool == true){
benson516 2:b9610a2d2ea0 56 if( reference_in > inputLimits_H){
weisnail 0:7f9b4ca968ae 57 reference = inputLimits_H;
benson516 2:b9610a2d2ea0 58 }else if( reference_in < inputLimits_L){
weisnail 0:7f9b4ca968ae 59 reference = inputLimits_L;
benson516 2:b9610a2d2ea0 60 }else{
benson516 2:b9610a2d2ea0 61 reference = reference_in;
weisnail 0:7f9b4ca968ae 62 }
benson516 2:b9610a2d2ea0 63
weisnail 0:7f9b4ca968ae 64 }else{
benson516 2:b9610a2d2ea0 65 reference = reference_in;
weisnail 0:7f9b4ca968ae 66 }
weisnail 0:7f9b4ca968ae 67
benson516 2:b9610a2d2ea0 68 // bypass
benson516 2:b9610a2d2ea0 69 feedbackvalue = feedbackvalue_in;
weisnail 0:7f9b4ca968ae 70
weisnail 0:7f9b4ca968ae 71 error[0] = reference - feedbackvalue;
benson516 2:b9610a2d2ea0 72 // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
benson516 2:b9610a2d2ea0 73 output = Kp*error[0] + Ki*error_I;
benson516 2:b9610a2d2ea0 74
benson516 2:b9610a2d2ea0 75 // Delay 1
benson516 2:b9610a2d2ea0 76 error[1] = error[0];
weisnail 0:7f9b4ca968ae 77
benson516 2:b9610a2d2ea0 78 // Integration
benson516 2:b9610a2d2ea0 79 error_I += Ts*error[0];
benson516 2:b9610a2d2ea0 80
benson516 2:b9610a2d2ea0 81 // Output satuation
benson516 2:b9610a2d2ea0 82 if(Outputlimit_bool && AntiWindUp_bool){
weisnail 0:7f9b4ca968ae 83 if( output >= outputLimits_H){
adam_z 1:4df4895863cd 84 output = output - (output - outputLimits_H)*Ka;
adam_z 1:4df4895863cd 85 //output = outputLimits_H;
weisnail 0:7f9b4ca968ae 86 }else if( output <= outputLimits_L){
benson516 2:b9610a2d2ea0 87 output = output - (output - outputLimits_L)*Ka;
adam_z 1:4df4895863cd 88 //output = outputLimits_L;
weisnail 0:7f9b4ca968ae 89 }
weisnail 0:7f9b4ca968ae 90 }else{
benson516 2:b9610a2d2ea0 91 // output = output;
weisnail 0:7f9b4ca968ae 92 }
weisnail 0:7f9b4ca968ae 93
adam_z 1:4df4895863cd 94 }
adam_z 1:4df4895863cd 95