Diff: PID.cpp

Revision:

4:e3c9cb64be44

Parent:

3:d8646d8c994f

Child:

5:016c99bb877f

--- a/PID.cpp	Tue Oct 25 14:19:13 2016 +0000
+++ b/PID.cpp	Wed Oct 26 17:56:01 2016 +0000
@@ -16,7 +16,8 @@
     Kp = Kp_in;
     Ki = Ki_in;
     Kd = Kd_in;
-    Ka = 0.1;
+    // Ka = 0.1; // Volt.-sec./deg.
+    Ka = 0.0017;// Volt.-sec./rad
     
     // Sampling time
     Ts = Sampletime_in; 
@@ -95,4 +96,64 @@
     }
             
 }
+void PID::Compute_noWindUP(float reference_in, float feedbackvalue_in){
 
+    if(Inputlimit_bool == true){
+        if( reference_in > inputLimits_H){
+            reference = inputLimits_H;
+        }else if( reference_in < inputLimits_L){
+            reference = inputLimits_L;
+        }else{
+            reference = reference_in;
+        }
+        
+    }else{
+        reference = reference_in;
+    }
+    
+    // bypass
+    feedbackvalue = feedbackvalue_in;
+    
+    error[0] = reference - feedbackvalue;
+    // 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];  
+    
+    // Integration
+    error_I += Ts*error[0];
+    
+    // Output satuation
+    /*
+    if(Outputlimit_bool && AntiWindUp_bool){
+        if( output >= outputLimits_H){
+            // output = output - (output - outputLimits_H)*Ka;
+            error_I -= Ka*(output - outputLimits_H); // Anti-windup
+            output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            // output = output - (output - outputLimits_L)*Ka;
+            error_I -= Ka*(output - outputLimits_L); // Anti-windup
+            output = outputLimits_L;
+        }
+    }else{
+        // output = output;
+    }
+    */
+            
+}
+void PID::Saturation_output(){
+        if( output >= outputLimits_H){
+            delta_output = outputLimits_H - output;
+            output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            delta_output = outputLimits_L - output;
+            output = outputLimits_L;
+        }else{
+            delta_output = 0.0;
+        }
+}
+void PID::Anti_windup(float delta){ // delta_V = Vs - V
+    // Anti-windup compensation
+    error_I += Ka*delta; // Anti-windup
+}
\ No newline at end of file