Diff: PID.cpp

Revision:

7:6f0e5de35b48

Parent:

6:0d1e877c7f60

--- a/PID.cpp	Thu Dec 15 20:17:49 2016 +0000
+++ b/PID.cpp	Thu Mar 30 07:52:35 2017 +0000
@@ -1,175 +1,274 @@
 #include "PID.h"
 
-PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in){
-    
+PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in):
+        derivative_error(Sampletime_in),
+        SAT_command(1,-1),
+        SAT_output(1,-1)
+{
+    // To enble, run this->start() function
+    enable = false;
+
+    // Sampling time
+    Ts = Sampletime_in;
+
+
+    // Parameters
+    is_limiting_command = false;
+    is_limiting_output = false; // true
+    //
+    is_using_integral = false; // Determine if the integral control is going to be used.
+    is_using_derivative = false; // Determine if the derivative control is going to be used.
+    //
+    is_using_outSource_d_error = false; // Determine whether using the signal for d_error or using numerical derivative to derive d_error from error.
+    //
+    is_antiWindUp = false; // true
+
     // Parameters
-    Outputlimit_bool = false; // true
-    Inputlimit_bool = false;
-    AntiWindUp_bool = false; // true
-    outputLimits_H = 7.4;
-    outputLimits_L = -7.4;
-    inputLimits_H = 0.0;
-    inputLimits_L = 0.0; 
-    feedbackvalue = 0.0; 
-    
-    // Feedback gain 
+    // Feedback gain
+    Kp = Kp_in;
+    Ki = Ki_in;
+    Kd = Kd_in;
+    //
+    if (Ki_in == 0.0){
+        is_using_integral = false;
+    }else{
+        is_using_integral = true;
+    }
+    //
+    if (Kd_in == 0.0){
+        is_using_derivative = false;
+    }else{
+        is_using_derivative = true;
+    }
+    //
+    // Ka = 0.0017;// Volt.-sec./rad
+    //
+    // Small over-bound value for numerical stability
+    overBound_value = 0.01; // Small positive value, adjustable
+
+
+    // States
+    error = 0.0;
+    d_error = 0.0;
+    error_int = 0.0;
+    //
+    error_int_increment = 0.0;
+
+    // Input signal
+    command = 0.0;
+    feedbackValue = 0.0;
+    // Output signal
+    output = 0.0;
+    // Error by saturation
+    delta_output = 0.0; // Error by saturation
+
+}
+// Process controller
+void PID::start(){ // Run
+    if (enable){
+        return;
+    }
+    //
+    enable = true;
+}
+void PID::pause(){ // Stop updating but no reset
+    //
+    enable = false;
+}
+void PID::stop(){ // Stop and reset
+    if (!enable){
+        return;
+    }
+    //
+    enable = false;
+    reset();
+}
+void PID::reset(void){
+    // States
+    error = 0.0;
+    d_error = 0.0;
+    error_int = 0.0;
+    //
+    error_int_increment = 0.0;
+
+    // Input signal
+    command = 0.0;
+    feedbackValue = 0.0;
+    // Output signal
+    output = 0.0;
+    // Error by saturation
+    delta_output = 0.0; // Error by saturation
+
+    // Reset the derivative
+    derivative_error.reset(0.0);
+}
+//
+void PID::EnableAntiWindUp(float Ka_in)
+{
+    is_antiWindUp = true;
+}
+//
+void PID::set_PID_gains(float Kp_in, float Ki_in, float Kd_in){ // Setting Kp, Ki, and Kd
     Kp = Kp_in;
     Ki = Ki_in;
     Kd = Kd_in;
-    // Ka = 0.1; // Volt.-sec./deg.
-    Ka = 0.0017;// Volt.-sec./rad
-    
-    // Sampling time
-    Ts = Sampletime_in; 
-    
-    // Variables
-    error[0] = 0.0;
-    error[1] = 0.0;
-    error_I = 0.0;
     //
-    reference = 0.0;
-    output = 0.0;
-     
-
-     
+    if (Ki_in == 0.0){
+        is_using_integral = false;
+    }else{
+        is_using_integral = true;
+    }
+    //
+    if (Kd_in == 0.0){
+        is_using_derivative = false;
+    }else{
+        is_using_derivative = true;
+    }
 }
-
-void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
-    Outputlimit_bool = true;
-    outputLimits_H = setoutputLimits_H;
-    outputLimits_L = setoutputLimits_L;    
+void PID::SetInputLimits(float inputLimits_H_in, float inputLimits_L_in){
+    is_limiting_command = true;
+    //
+    SAT_command.set_bound(inputLimits_H_in, inputLimits_L_in);
 }
+void PID::SetOutputLimits(float outputLimits_H_in, float outputLimits_L_in){
+    is_limiting_output = true;
+    // is_antiWindUp = true;
+    //
+    SAT_output.set_bound(outputLimits_H_in, outputLimits_L_in);
 
-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_in)
-{
-    AntiWindUp_bool = true;
-    Ka = Ka_in;
+    //
+    // Set the over-bound value to be 1% of peak-peak range
+    overBound_value = 0.001*(outputLimits_H_in - outputLimits_L_in);
 }
 
-void PID::Compute(float reference_in, float feedbackvalue_in){
+// Main function for computing the PID
+//--------------------------------------------------//
+void PID::set_d_error(float d_error_in){ // Insert d_error before iteration.
+    d_error = d_error_in;
+    is_using_outSource_d_error = true;
+}
+//
+void PID::iterateOnce(float command_in, float feedbackValue_in){
+    // Main process
+    iterateOnce_noAntiWindUP(command_in, feedbackValue_in);
 
-    if(Inputlimit_bool == true){
-        if( reference_in > inputLimits_H){
-            reference = inputLimits_H;
-        }else if( reference_in < inputLimits_L){
-            reference = inputLimits_L;
+    // Output satuation
+    if(is_limiting_output){
+        if (is_antiWindUp){
+            // Output saturation + anti-windup
+            this->Saturation_AntiWindUp();
         }else{
-            reference = reference_in;
+            // Output saturation only
+            this->Saturation_output();
         }
-        
-    }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::Compute_noWindUP(float reference_in, float feedbackvalue_in){
+//
+void PID::iterateOnce_noAntiWindUP(float command_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;
-        }
-        
+    //
+    // -- Important! --
+    // Post-integral action:
+    // This integral action generates the error_int of time (k-1) (previous time), which means the back-step integral.
+    // The actual integral action move to here is for implementing the (digital-version) anti-windup.
+    if (is_using_integral){
+        error_int += error_int_increment;
     }else{
-        reference = reference_in;
+        error_int = 0.0;
+    }
+    //
+
+    // Processing input signals
+    //----------------------------------------//
+    // Comand saturation
+    if(is_limiting_command){
+        // Saturation
+        command = SAT_command.filter(command_in);
+        //
+    }else{
+        command = command_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;
+
+    // bypass the feedback value
+    feedbackValue = feedbackValue_in;
+    //----------------------------------------//
+
+    // PID control
+    //----------------------------------------//
+    // Calculating the error
+    error = command - feedbackValue;
+    //
+    if (is_using_derivative){
+        if (is_using_outSource_d_error){
+            // Please use the insert function for d_error.
+        }else{
+            // Calculating the derivative of error
+            d_error = derivative_error.filter(error);
         }
+        // Control output
+        output = Kp*error + Ki*error_int + Kd*d_error;
     }else{
-        // output = output;
+        // Control output
+        output = Kp*error + Ki*error_int;
     }
-    */
-            
+    //----------------------------------------//
+
+    // Pre-integral action
+    error_int_increment = Ts*error;
+
 }
+//--------------------------------------------------//
 
-// Use separately
-// Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- Anti_windup(delta) -- output
+
+// Used separately
+// Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- AntiWindUp(delta) -- 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;
+    //
+    output = SAT_output.filter(output);
+    delta_output = SAT_output.delta_out; // (original_out - limited_out)
+}
+void PID::AntiWindUp(float delta){ // delta_V = V - V_sat
+
+    /*
+    // (Analog) Anti-windup compensation
+    // error_int -= Ka*delta; // Anti-windup
+    error_int_increment -= Ka*delta; // Anti-windup
+    */
+
+    // (Digital) Anti-windup
+    // If the output is going to be over bound, stop the integral action in that direction
+    if (Ki > 0.0){
+        //
+        if (delta > overBound_value && error_int_increment > 0.0){ // Positive saturation
+            error_int_increment = 0.0;
+        }else if (delta < -overBound_value && error_int_increment < 0.0){ // Negative saturation
+            error_int_increment = 0.0;
         }
-}
-void PID::Anti_windup(float delta){ // delta_V = Vs - V
-    // Anti-windup compensation
-    error_I += Ka*delta; // Anti-windup
+    }else if (Ki < 0.0){
+        //
+        if (delta > overBound_value && error_int_increment < 0.0){ // Positive saturation
+            error_int_increment = 0.0;
+        }else if (delta < -overBound_value && error_int_increment > 0.0){ // Negative saturation
+            error_int_increment = 0.0;
+        }
+    }
+
 }
 ////////////////////////////////////////////////// end Use separately
 
-// Use alone
-// Compute_noWindUP() -- Anti_windup() -- output
+// Used alone
+// Compute_noWindUP() -- Saturation_AntiWindUp() -- output
 //////////////////////////////////////////////////
-void PID::Anti_windup(){ // delta_V = Vs - V
-
-    PID::Saturation_output();
+void PID::Saturation_AntiWindUp(){ // delta_V = V - V_sat
+    //
+    // Output saturation
+    this->Saturation_output();
     // Anti-windup compensation
-    error_I += Ka*PID::delta_output; // Anti-windup
+    this->AntiWindUp(delta_output);
 }
-////////////////////////////////////////////////// end Use alone
\ No newline at end of file
+////////////////////////////////////////////////// end Use alone