Diff: PID.cpp

Revision:

0:7f9b4ca968ae

Child:

1:4df4895863cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Thu Feb 18 04:25:52 2016 +0000
@@ -0,0 +1,69 @@
+#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;
+    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::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){
+        if( output >= outputLimits_H){
+            output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            output = outputLimits_L;
+        }
+    }else{
+        output = output;
+    }
+            
+}
\ No newline at end of file