LI SU / PID

ROS_FINGER

Fork of PID by LDSC_Robotics_TAs

PID.cpp@2:d1b781222f88, 2018-08-22 (annotated)

Committer:
noname001
Date:
Wed Aug 22 08:09:53 2018 +0000
Revision:
2:d1b781222f88
Parent:
1:4df4895863cd 
ROS

Who changed what in which revision?

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