LI SU / PID

ROS_FINGER

Fork of PID by LDSC_Robotics_TAs

PID.cpp@0:7f9b4ca968ae, 2016-02-18 (annotated)

Committer:
weisnail
Date:
Thu Feb 18 04:25:52 2016 +0000
Revision:
0:7f9b4ca968ae
Child:
1:4df4895863cd
add PID library

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;
weisnail 0:7f9b4ca968ae 16 outputLimits_H = 0.0;
weisnail 0:7f9b4ca968ae 17 outputLimits_L = 0.0;
weisnail 0:7f9b4ca968ae 18 inputLimits_H = 0.0;
weisnail 0:7f9b4ca968ae 19 inputLimits_L = 0.0;
weisnail 0:7f9b4ca968ae 20 feedbackvalue = 0.0;
weisnail 0:7f9b4ca968ae 21
weisnail 0:7f9b4ca968ae 22 Kp = setKp;
weisnail 0:7f9b4ca968ae 23 Ki = setKi/setSampletime;
weisnail 0:7f9b4ca968ae 24 Kd = setKd*setSampletime;
weisnail 0:7f9b4ca968ae 25 sampletime = setSampletime;
weisnail 0:7f9b4ca968ae 26 }
weisnail 0:7f9b4ca968ae 27
weisnail 0:7f9b4ca968ae 28 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
weisnail 0:7f9b4ca968ae 29 Outputlimit_bool = true;
weisnail 0:7f9b4ca968ae 30 outputLimits_H = setoutputLimits_H;
weisnail 0:7f9b4ca968ae 31 outputLimits_L = setoutputLimits_L;
weisnail 0:7f9b4ca968ae 32 }
weisnail 0:7f9b4ca968ae 33
weisnail 0:7f9b4ca968ae 34 void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
weisnail 0:7f9b4ca968ae 35 Inputlimit_bool = true;
weisnail 0:7f9b4ca968ae 36 inputLimits_H = setinputLimits_H;
weisnail 0:7f9b4ca968ae 37 inputLimits_L = setinputLimits_L;
weisnail 0:7f9b4ca968ae 38 }
weisnail 0:7f9b4ca968ae 39
weisnail 0:7f9b4ca968ae 40 void PID::Compute(float setreference, float setfeedbackvalue){
weisnail 0:7f9b4ca968ae 41
weisnail 0:7f9b4ca968ae 42 if(Inputlimit_bool == true){
weisnail 0:7f9b4ca968ae 43 if( setreference >= inputLimits_H){
weisnail 0:7f9b4ca968ae 44 reference = inputLimits_H;
weisnail 0:7f9b4ca968ae 45 }else if( setreference <= inputLimits_L){
weisnail 0:7f9b4ca968ae 46 reference = inputLimits_L;
weisnail 0:7f9b4ca968ae 47 }
weisnail 0:7f9b4ca968ae 48 }else{
weisnail 0:7f9b4ca968ae 49 reference = setreference;
weisnail 0:7f9b4ca968ae 50 }
weisnail 0:7f9b4ca968ae 51
weisnail 0:7f9b4ca968ae 52 feedbackvalue = setfeedbackvalue;
weisnail 0:7f9b4ca968ae 53
weisnail 0:7f9b4ca968ae 54 error[0] = reference - feedbackvalue;
weisnail 0:7f9b4ca968ae 55 output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
weisnail 0:7f9b4ca968ae 56 error[2] = error[1];
weisnail 0:7f9b4ca968ae 57 error[1] = error[0];
weisnail 0:7f9b4ca968ae 58
weisnail 0:7f9b4ca968ae 59 if(Outputlimit_bool == true){
weisnail 0:7f9b4ca968ae 60 if( output >= outputLimits_H){
weisnail 0:7f9b4ca968ae 61 output = outputLimits_H;
weisnail 0:7f9b4ca968ae 62 }else if( output <= outputLimits_L){
weisnail 0:7f9b4ca968ae 63 output = outputLimits_L;
weisnail 0:7f9b4ca968ae 64 }
weisnail 0:7f9b4ca968ae 65 }else{
weisnail 0:7f9b4ca968ae 66 output = output;
weisnail 0:7f9b4ca968ae 67 }
weisnail 0:7f9b4ca968ae 68
weisnail 0:7f9b4ca968ae 69 }