Diff: PID.cpp

Revision:

2:b9610a2d2ea0

Parent:

1:4df4895863cd

Child:

3:d8646d8c994f

--- a/PID.cpp	Fri Apr 22 09:38:38 2016 +0000
+++ b/PID.cpp	Tue Oct 25 13:04:58 2016 +0000
@@ -1,16 +1,8 @@
 #include "PID.h"
 
-PID::PID(float setKp, float setKi, float setKd,  float setSampletime){
+PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in){
     
-    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;
+    // Parameters
     Outputlimit_bool = false;
     Inputlimit_bool = false;
     AntiWindUp_bool = false;
@@ -18,12 +10,26 @@
     outputLimits_L = 0.0;
     inputLimits_H = 0.0;
     inputLimits_L = 0.0; 
-    feedbackvalue = 0.0;   
+    feedbackvalue = 0.0; 
+    
+    // Feedback gain 
+    Kp = Kp_in;
+    Ki = Ki_in;
+    Kd = Kd_in;
+    
+    // Sampling time
+    Ts = Sampletime_in; 
     
-    Kp = setKp;
-    Ki = setKi*setSampletime;
-    Kd = setKd/setSampletime;
-    sampletime = setSampletime;   
+    // Variables
+    error[0] = 0.0;
+    error[1] = 0.0;
+    error_I = 0.0;
+    //
+    reference = 0.0;
+    output = 0.0;
+     
+
+     
 }
 
 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
@@ -44,36 +50,45 @@
     Ka = Ka_;
 }
 
-void PID::Compute(float setreference, float setfeedbackvalue){
+void PID::Compute(float reference_in, float feedbackvalue_in){
 
     if(Inputlimit_bool == true){
-        if( setreference >= inputLimits_H){
+        if( reference_in > inputLimits_H){
             reference = inputLimits_H;
-        }else if( setreference <= inputLimits_L){
+        }else if( reference_in < inputLimits_L){
             reference = inputLimits_L;
+        }else{
+            reference = reference_in;
         }
+        
     }else{
-        reference = setreference;
+        reference = reference_in;
     }
     
-    feedbackvalue = setfeedbackvalue;
+    // bypass
+    feedbackvalue = feedbackvalue_in;
     
     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];    
+    // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
+    output = Kp*error[0] + Ki*error_I;
+    
+    // Delay 1
+    error[1] = error[0];  
     
-    if(Outputlimit_bool == true && AntiWindUp_bool == true){
+    // Integration
+    error_I += Ts*error[0];
+    
+    // Output satuation
+    if(Outputlimit_bool && AntiWindUp_bool){
         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 = output - (output - outputLimits_L)*Ka;
             //output = outputLimits_L;
-            
         }
     }else{
-        output = output;
+        // output = output;
     }
             
 }