Vito Lin / PID

Hello

PID.cpp@0:004c04a30fce, 2018-11-22 (annotated)

Committer:
ChengHan
Date:
Thu Nov 22 01:07:08 2018 +0000
Revision:
0:004c04a30fce
Hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ChengHan 0:004c04a30fce 1 #include "PID.h"
ChengHan 0:004c04a30fce 2
ChengHan 0:004c04a30fce 3 PID::PID(float setKp, float setKi, float setKd, float setSampletime){
ChengHan 0:004c04a30fce 4
ChengHan 0:004c04a30fce 5 Kp = 0.0;
ChengHan 0:004c04a30fce 6 Ki = 0.0;
ChengHan 0:004c04a30fce 7 Kd = 0.0;
ChengHan 0:004c04a30fce 8 error[0] = 0.0;
ChengHan 0:004c04a30fce 9 error[1] = 0.0;
ChengHan 0:004c04a30fce 10 error[2] = 0.0;
ChengHan 0:004c04a30fce 11 output = 0.0;
ChengHan 0:004c04a30fce 12 reference = 0.0;
ChengHan 0:004c04a30fce 13 sampletime = 0.0;
ChengHan 0:004c04a30fce 14 Outputlimit_bool = false;
ChengHan 0:004c04a30fce 15 Inputlimit_bool = false;
ChengHan 0:004c04a30fce 16 AntiWindUp_bool = false;
ChengHan 0:004c04a30fce 17 outputLimits_H = 0.0;
ChengHan 0:004c04a30fce 18 outputLimits_L = 0.0;
ChengHan 0:004c04a30fce 19 inputLimits_H = 0.0;
ChengHan 0:004c04a30fce 20 inputLimits_L = 0.0;
ChengHan 0:004c04a30fce 21 feedbackvalue = 0.0;
ChengHan 0:004c04a30fce 22
ChengHan 0:004c04a30fce 23 Kp = setKp;
ChengHan 0:004c04a30fce 24 Ki = setKi*setSampletime;
ChengHan 0:004c04a30fce 25 Kd = setKd/setSampletime;
ChengHan 0:004c04a30fce 26 sampletime = setSampletime;
ChengHan 0:004c04a30fce 27 }
ChengHan 0:004c04a30fce 28
ChengHan 0:004c04a30fce 29 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
ChengHan 0:004c04a30fce 30 Outputlimit_bool = true;
ChengHan 0:004c04a30fce 31 outputLimits_H = setoutputLimits_H;
ChengHan 0:004c04a30fce 32 outputLimits_L = setoutputLimits_L;
ChengHan 0:004c04a30fce 33 }
ChengHan 0:004c04a30fce 34
ChengHan 0:004c04a30fce 35 void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
ChengHan 0:004c04a30fce 36 Inputlimit_bool = true;
ChengHan 0:004c04a30fce 37 inputLimits_H = setinputLimits_H;
ChengHan 0:004c04a30fce 38 inputLimits_L = setinputLimits_L;
ChengHan 0:004c04a30fce 39 }
ChengHan 0:004c04a30fce 40
ChengHan 0:004c04a30fce 41 void PID::EnableAntiWindUp(float Ka_)
ChengHan 0:004c04a30fce 42 {
ChengHan 0:004c04a30fce 43 AntiWindUp_bool = true;
ChengHan 0:004c04a30fce 44 Ka = Ka_;
ChengHan 0:004c04a30fce 45 }
ChengHan 0:004c04a30fce 46
ChengHan 0:004c04a30fce 47 void PID::Compute(float setreference, float setfeedbackvalue){
ChengHan 0:004c04a30fce 48
ChengHan 0:004c04a30fce 49 if(Inputlimit_bool == true){
ChengHan 0:004c04a30fce 50 if( setreference >= inputLimits_H){
ChengHan 0:004c04a30fce 51 reference = inputLimits_H;
ChengHan 0:004c04a30fce 52 }else if( setreference <= inputLimits_L){
ChengHan 0:004c04a30fce 53 reference = inputLimits_L;
ChengHan 0:004c04a30fce 54 }
ChengHan 0:004c04a30fce 55 }else{
ChengHan 0:004c04a30fce 56 reference = setreference;
ChengHan 0:004c04a30fce 57 }
ChengHan 0:004c04a30fce 58
ChengHan 0:004c04a30fce 59 feedbackvalue = setfeedbackvalue;
ChengHan 0:004c04a30fce 60
ChengHan 0:004c04a30fce 61 error[0] = reference - feedbackvalue;
ChengHan 0:004c04a30fce 62 output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
ChengHan 0:004c04a30fce 63 error[2] = error[1];
ChengHan 0:004c04a30fce 64 error[1] = error[0];
ChengHan 0:004c04a30fce 65
ChengHan 0:004c04a30fce 66 if(Outputlimit_bool == true && AntiWindUp_bool == true){
ChengHan 0:004c04a30fce 67 if( output >= outputLimits_H){
ChengHan 0:004c04a30fce 68 output = output - (output - outputLimits_H)*Ka;
ChengHan 0:004c04a30fce 69 //output = outputLimits_H;
ChengHan 0:004c04a30fce 70 }else if( output <= outputLimits_L){
ChengHan 0:004c04a30fce 71 output =output - (output - outputLimits_L)*Ka;
ChengHan 0:004c04a30fce 72 //output = outputLimits_L;
ChengHan 0:004c04a30fce 73
ChengHan 0:004c04a30fce 74 }
ChengHan 0:004c04a30fce 75 }else{
ChengHan 0:004c04a30fce 76 output = output;
ChengHan 0:004c04a30fce 77 }
ChengHan 0:004c04a30fce 78
ChengHan 0:004c04a30fce 79 }
ChengHan 0:004c04a30fce 80