PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float setKp, float setKi, float setKd,  float setSampletime){
00004     
00005     Kp = 0.0;
00006     Ki = 0.0;
00007     Kd = 0.0;
00008     error[0] = 0.0;
00009     error[1] = 0.0;
00010     error[2] = 0.0;
00011     output = 0.0;
00012 //    pidOutput = 0.0;
00013     reference = 0.0;
00014     sampletime = 0.0;
00015     Outputlimit_bool = false;
00016     Inputlimit_bool = false;
00017     AntiWindUp_bool = false;
00018     outputLimits_H = 0.0;
00019     outputLimits_L = 0.0;
00020     inputLimits_H = 0.0;
00021     inputLimits_L = 0.0; 
00022     feedbackvalue = 0.0;   
00023     
00024     Kp = setKp;
00025     Ki = setKi*setSampletime;
00026     Kd = setKd/setSampletime;
00027     sampletime = setSampletime;   
00028 }
00029 
00030 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
00031     Outputlimit_bool = true;
00032     outputLimits_H = setoutputLimits_H;
00033     outputLimits_L = setoutputLimits_L;    
00034 }
00035 
00036 void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
00037     Inputlimit_bool = true;
00038     inputLimits_H = setinputLimits_H;
00039     inputLimits_L = setinputLimits_L;     
00040 }
00041 
00042 void PID::EnableAntiWindUp(float Ka_)
00043 {
00044     AntiWindUp_bool = true;
00045     Ka = Ka_;
00046 }
00047 
00048 void PID::Compute(float setreference, float setfeedbackvalue){
00049 
00050     if(Inputlimit_bool == true){
00051         if( setreference >= inputLimits_H){
00052             reference = inputLimits_H;
00053         }else if( setreference <= inputLimits_L){
00054             reference = inputLimits_L;
00055         }
00056     }else{
00057         reference = setreference;
00058     }
00059     
00060     feedbackvalue = setfeedbackvalue;
00061     
00062     error[0] = reference - feedbackvalue;
00063 //    pidOutput = ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2];
00064     output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2];
00065     error[2] = error[1];
00066     error[1] = error[0];    
00067     
00068     if(Outputlimit_bool == true && AntiWindUp_bool == true){
00069         if( output >= outputLimits_H){
00070             output = output - (output - outputLimits_H)*Ka;
00071             //output = outputLimits_H;
00072         }
00073         else if( output <= outputLimits_L){
00074             output =output - (output - outputLimits_L)*Ka;
00075             //output = outputLimits_L;  
00076         }
00077     }else{
00078         output = output;
00079     }
00080             
00081 }
00082