PID
Fork of PID by
PID.cpp@2:f8864ce26cd8, 2017-11-21 (annotated)
- Committer:
- roger5641
- Date:
- Tue Nov 21 05:55:11 2017 +0000
- Revision:
- 2:f8864ce26cd8
- Parent:
- 1:4df4895863cd
- Child:
- 3:dccd51230bf5
no change
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
weisnail | 0:7f9b4ca968ae | 1 | #include "PID.h" |
weisnail | 0:7f9b4ca968ae | 2 | |
weisnail | 0:7f9b4ca968ae | 3 | PID::PID(float setKp, float setKi, float setKd, float setSampletime){ |
weisnail | 0:7f9b4ca968ae | 4 | |
weisnail | 0:7f9b4ca968ae | 5 | Kp = 0.0; |
weisnail | 0:7f9b4ca968ae | 6 | Ki = 0.0; |
weisnail | 0:7f9b4ca968ae | 7 | Kd = 0.0; |
weisnail | 0:7f9b4ca968ae | 8 | error[0] = 0.0; |
weisnail | 0:7f9b4ca968ae | 9 | error[1] = 0.0; |
weisnail | 0:7f9b4ca968ae | 10 | error[2] = 0.0; |
weisnail | 0:7f9b4ca968ae | 11 | output = 0.0; |
roger5641 | 2:f8864ce26cd8 | 12 | // pidOutput = 0.0; |
weisnail | 0:7f9b4ca968ae | 13 | reference = 0.0; |
weisnail | 0:7f9b4ca968ae | 14 | sampletime = 0.0; |
weisnail | 0:7f9b4ca968ae | 15 | Outputlimit_bool = false; |
weisnail | 0:7f9b4ca968ae | 16 | Inputlimit_bool = false; |
adam_z | 1:4df4895863cd | 17 | AntiWindUp_bool = false; |
weisnail | 0:7f9b4ca968ae | 18 | outputLimits_H = 0.0; |
weisnail | 0:7f9b4ca968ae | 19 | outputLimits_L = 0.0; |
weisnail | 0:7f9b4ca968ae | 20 | inputLimits_H = 0.0; |
weisnail | 0:7f9b4ca968ae | 21 | inputLimits_L = 0.0; |
weisnail | 0:7f9b4ca968ae | 22 | feedbackvalue = 0.0; |
weisnail | 0:7f9b4ca968ae | 23 | |
weisnail | 0:7f9b4ca968ae | 24 | Kp = setKp; |
adam_z | 1:4df4895863cd | 25 | Ki = setKi*setSampletime; |
adam_z | 1:4df4895863cd | 26 | Kd = setKd/setSampletime; |
weisnail | 0:7f9b4ca968ae | 27 | sampletime = setSampletime; |
weisnail | 0:7f9b4ca968ae | 28 | } |
weisnail | 0:7f9b4ca968ae | 29 | |
weisnail | 0:7f9b4ca968ae | 30 | void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){ |
weisnail | 0:7f9b4ca968ae | 31 | Outputlimit_bool = true; |
weisnail | 0:7f9b4ca968ae | 32 | outputLimits_H = setoutputLimits_H; |
weisnail | 0:7f9b4ca968ae | 33 | outputLimits_L = setoutputLimits_L; |
weisnail | 0:7f9b4ca968ae | 34 | } |
weisnail | 0:7f9b4ca968ae | 35 | |
weisnail | 0:7f9b4ca968ae | 36 | void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){ |
weisnail | 0:7f9b4ca968ae | 37 | Inputlimit_bool = true; |
weisnail | 0:7f9b4ca968ae | 38 | inputLimits_H = setinputLimits_H; |
weisnail | 0:7f9b4ca968ae | 39 | inputLimits_L = setinputLimits_L; |
weisnail | 0:7f9b4ca968ae | 40 | } |
weisnail | 0:7f9b4ca968ae | 41 | |
adam_z | 1:4df4895863cd | 42 | void PID::EnableAntiWindUp(float Ka_) |
adam_z | 1:4df4895863cd | 43 | { |
adam_z | 1:4df4895863cd | 44 | AntiWindUp_bool = true; |
adam_z | 1:4df4895863cd | 45 | Ka = Ka_; |
adam_z | 1:4df4895863cd | 46 | } |
adam_z | 1:4df4895863cd | 47 | |
weisnail | 0:7f9b4ca968ae | 48 | void PID::Compute(float setreference, float setfeedbackvalue){ |
weisnail | 0:7f9b4ca968ae | 49 | |
weisnail | 0:7f9b4ca968ae | 50 | if(Inputlimit_bool == true){ |
weisnail | 0:7f9b4ca968ae | 51 | if( setreference >= inputLimits_H){ |
weisnail | 0:7f9b4ca968ae | 52 | reference = inputLimits_H; |
weisnail | 0:7f9b4ca968ae | 53 | }else if( setreference <= inputLimits_L){ |
weisnail | 0:7f9b4ca968ae | 54 | reference = inputLimits_L; |
weisnail | 0:7f9b4ca968ae | 55 | } |
weisnail | 0:7f9b4ca968ae | 56 | }else{ |
weisnail | 0:7f9b4ca968ae | 57 | reference = setreference; |
weisnail | 0:7f9b4ca968ae | 58 | } |
weisnail | 0:7f9b4ca968ae | 59 | |
weisnail | 0:7f9b4ca968ae | 60 | feedbackvalue = setfeedbackvalue; |
weisnail | 0:7f9b4ca968ae | 61 | |
weisnail | 0:7f9b4ca968ae | 62 | error[0] = reference - feedbackvalue; |
roger5641 | 2:f8864ce26cd8 | 63 | // pidOutput = ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2]; |
roger5641 | 2:f8864ce26cd8 | 64 | output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2]; |
weisnail | 0:7f9b4ca968ae | 65 | error[2] = error[1]; |
weisnail | 0:7f9b4ca968ae | 66 | error[1] = error[0]; |
weisnail | 0:7f9b4ca968ae | 67 | |
adam_z | 1:4df4895863cd | 68 | if(Outputlimit_bool == true && AntiWindUp_bool == true){ |
weisnail | 0:7f9b4ca968ae | 69 | if( output >= outputLimits_H){ |
adam_z | 1:4df4895863cd | 70 | output = output - (output - outputLimits_H)*Ka; |
adam_z | 1:4df4895863cd | 71 | //output = outputLimits_H; |
weisnail | 0:7f9b4ca968ae | 72 | }else if( output <= outputLimits_L){ |
adam_z | 1:4df4895863cd | 73 | output =output - (output - outputLimits_L)*Ka; |
adam_z | 1:4df4895863cd | 74 | //output = outputLimits_L; |
adam_z | 1:4df4895863cd | 75 | |
weisnail | 0:7f9b4ca968ae | 76 | } |
weisnail | 0:7f9b4ca968ae | 77 | }else{ |
weisnail | 0:7f9b4ca968ae | 78 | output = output; |
weisnail | 0:7f9b4ca968ae | 79 | } |
weisnail | 0:7f9b4ca968ae | 80 | |
adam_z | 1:4df4895863cd | 81 | } |
adam_z | 1:4df4895863cd | 82 |