Roger Weng
PID
Fork of
PID
by 
LDSC_Robotics_TAs
PID.cpp@3:dccd51230bf5, 2018-04-12 (annotated)
- Committer:
- roger5641
- Date:
- Thu Apr 12 11:38:23 2018 +0000
- Revision:
- 3:dccd51230bf5
- Parent:
- 2:f8864ce26cd8
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; |
| roger5641 | 3:dccd51230bf5 | 72 | } |
| roger5641 | 3:dccd51230bf5 | 73 | else if( output <= outputLimits_L){ |
| adam_z | 1:4df4895863cd | 74 | output =output - (output - outputLimits_L)*Ka; |
| roger5641 | 3:dccd51230bf5 | 75 | //output = outputLimits_L; |
| 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 |