Diff: PID.cpp

Revision:

0:d2c5f7d65d0d

diff -r 000000000000 -r d2c5f7d65d0d PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Fri Nov 23 04:43:14 2018 +0000
@@ -0,0 +1,79 @@
+#include "PID.h"
+
+PID::PID(float setKp, float setKi, float setKd,  float setSampletime){
+    
+    Kp = 0.0;
+    Ki = 0.0;
+    Kd = 0.0;
+    error[0] = 0.0;
+    error[1] = 0.0;
+    error[2] = 0.0;
+    output = 0.0;
+    reference = 0.0;
+    sampletime = 0.0;
+    Outputlimit_bool = false;
+    Inputlimit_bool = false;
+    AntiWindUp_bool = false;
+    outputLimits_H = 0.0;
+    outputLimits_L = 0.0;
+    inputLimits_H = 0.0;
+    inputLimits_L = 0.0; 
+    feedbackvalue = 0.0;   
+    
+    Kp = setKp;
+    Ki = setKi*setSampletime;
+    Kd = setKd/setSampletime;
+    sampletime = setSampletime;   
+}
+
+void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
+    Outputlimit_bool = true;
+    outputLimits_H = setoutputLimits_H;
+    outputLimits_L = setoutputLimits_L;    
+}
+
+void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
+    Inputlimit_bool = true;
+    inputLimits_H = setinputLimits_H;
+    inputLimits_L = setinputLimits_L;     
+}
+
+void PID::EnableAntiWindUp(float Ka_)
+{
+    AntiWindUp_bool = true;
+    Ka = Ka_;
+}
+
+void PID::Compute(float setreference, float setfeedbackvalue){
+
+    if(Inputlimit_bool == true){
+        if( setreference >= inputLimits_H){
+            reference = inputLimits_H;
+        }else if( setreference <= inputLimits_L){
+            reference = inputLimits_L;
+        }
+    }else{
+        reference = setreference;
+    }
+    
+    feedbackvalue = setfeedbackvalue;
+    
+    error[0] = reference - feedbackvalue;
+    output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
+    error[2] = error[1];
+    error[1] = error[0];    
+    
+    if(Outputlimit_bool == true && AntiWindUp_bool == true){
+        if( output >= outputLimits_H){
+            output = output - (output - outputLimits_H)*Ka;
+            //output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            output =output - (output - outputLimits_L)*Ka;
+            //output = outputLimits_L;
+            
+        }
+    }else{
+        output = output;
+    }
+            
+}
\ No newline at end of file