New PID library with digital anti-windup and process control

Fork of PID_modified by Chun Feng Huang

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers PID.cpp Source File

PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in):
00004         derivative_error(Sampletime_in),
00005         SAT_command(1,-1),
00006         SAT_output(1,-1)
00007 {
00008     // To enble, run this->start() function
00009     enable = false;
00010 
00011     // Sampling time
00012     Ts = Sampletime_in;
00013 
00014 
00015     // Parameters
00016     is_limiting_command = false;
00017     is_limiting_output = false; // true
00018     //
00019     is_using_integral = false; // Determine if the integral control is going to be used.
00020     is_using_derivative = false; // Determine if the derivative control is going to be used.
00021     //
00022     is_using_outSource_d_error = false; // Determine whether using the signal for d_error or using numerical derivative to derive d_error from error.
00023     //
00024     is_antiWindUp = false; // true
00025 
00026     // Parameters
00027     // Feedback gain
00028     Kp = Kp_in;
00029     Ki = Ki_in;
00030     Kd = Kd_in;
00031     //
00032     if (Ki_in == 0.0){
00033         is_using_integral = false;
00034     }else{
00035         is_using_integral = true;
00036     }
00037     //
00038     if (Kd_in == 0.0){
00039         is_using_derivative = false;
00040     }else{
00041         is_using_derivative = true;
00042     }
00043     //
00044     // Ka = 0.0017;// Volt.-sec./rad
00045     //
00046     // Small over-bound value for numerical stability
00047     overBound_value = 0.01; // Small positive value, adjustable
00048 
00049 
00050     // States
00051     error = 0.0;
00052     d_error = 0.0;
00053     error_int = 0.0;
00054     //
00055     error_int_increment = 0.0;
00056 
00057     // Input signal
00058     command = 0.0;
00059     feedbackValue = 0.0;
00060     // Output signal
00061     output = 0.0;
00062     // Error by saturation
00063     delta_output = 0.0; // Error by saturation
00064 
00065 }
00066 // Process controller
00067 void PID::start(){ // Run
00068     if (enable){
00069         return;
00070     }
00071     //
00072     enable = true;
00073 }
00074 void PID::pause(){ // Stop updating but no reset
00075     //
00076     enable = false;
00077 }
00078 void PID::stop(){ // Stop and reset
00079     if (!enable){
00080         return;
00081     }
00082     //
00083     enable = false;
00084     reset();
00085 }
00086 void PID::reset(void){
00087     // States
00088     error = 0.0;
00089     d_error = 0.0;
00090     error_int = 0.0;
00091     //
00092     error_int_increment = 0.0;
00093 
00094     // Input signal
00095     command = 0.0;
00096     feedbackValue = 0.0;
00097     // Output signal
00098     output = 0.0;
00099     // Error by saturation
00100     delta_output = 0.0; // Error by saturation
00101 
00102     // Reset the derivative
00103     derivative_error.reset(0.0);
00104 }
00105 //
00106 void PID::EnableAntiWindUp(float Ka_in)
00107 {
00108     is_antiWindUp = true;
00109 }
00110 //
00111 void PID::set_PID_gains(float Kp_in, float Ki_in, float Kd_in){ // Setting Kp, Ki, and Kd
00112     Kp = Kp_in;
00113     Ki = Ki_in;
00114     Kd = Kd_in;
00115     //
00116     if (Ki_in == 0.0){
00117         is_using_integral = false;
00118     }else{
00119         is_using_integral = true;
00120     }
00121     //
00122     if (Kd_in == 0.0){
00123         is_using_derivative = false;
00124     }else{
00125         is_using_derivative = true;
00126     }
00127 }
00128 void PID::SetInputLimits(float inputLimits_H_in, float inputLimits_L_in){
00129     is_limiting_command = true;
00130     //
00131     SAT_command.set_bound(inputLimits_H_in, inputLimits_L_in);
00132 }
00133 void PID::SetOutputLimits(float outputLimits_H_in, float outputLimits_L_in){
00134     is_limiting_output = true;
00135     // is_antiWindUp = true;
00136     //
00137     SAT_output.set_bound(outputLimits_H_in, outputLimits_L_in);
00138 
00139     //
00140     // Set the over-bound value to be 1% of peak-peak range
00141     overBound_value = 0.001*(outputLimits_H_in - outputLimits_L_in);
00142 }
00143 
00144 // Main function for computing the PID
00145 //--------------------------------------------------//
00146 void PID::set_d_error(float d_error_in){ // Insert d_error before iteration.
00147     d_error = d_error_in;
00148     is_using_outSource_d_error = true;
00149 }
00150 //
00151 void PID::iterateOnce(float command_in, float feedbackValue_in){
00152     // Main process
00153     iterateOnce_noAntiWindUP(command_in, feedbackValue_in);
00154 
00155     // Output satuation
00156     if(is_limiting_output){
00157         if (is_antiWindUp){
00158             // Output saturation + anti-windup
00159             this->Saturation_AntiWindUp();
00160         }else{
00161             // Output saturation only
00162             this->Saturation_output();
00163         }
00164         //
00165     }else{
00166         // output = output;
00167     }
00168 
00169 }
00170 //
00171 void PID::iterateOnce_noAntiWindUP(float command_in, float feedbackValue_in){
00172 
00173     //
00174     // -- Important! --
00175     // Post-integral action:
00176     // This integral action generates the error_int of time (k-1) (previous time), which means the back-step integral.
00177     // The actual integral action move to here is for implementing the (digital-version) anti-windup.
00178     if (is_using_integral){
00179         error_int += error_int_increment;
00180     }else{
00181         error_int = 0.0;
00182     }
00183     //
00184 
00185     // Processing input signals
00186     //----------------------------------------//
00187     // Comand saturation
00188     if(is_limiting_command){
00189         // Saturation
00190         command = SAT_command.filter(command_in);
00191         //
00192     }else{
00193         command = command_in;
00194     }
00195 
00196     // bypass the feedback value
00197     feedbackValue = feedbackValue_in;
00198     //----------------------------------------//
00199 
00200     // PID control
00201     //----------------------------------------//
00202     // Calculating the error
00203     error = command - feedbackValue;
00204     //
00205     if (is_using_derivative){
00206         if (is_using_outSource_d_error){
00207             // Please use the insert function for d_error.
00208         }else{
00209             // Calculating the derivative of error
00210             d_error = derivative_error.filter(error);
00211         }
00212         // Control output
00213         output = Kp*error + Ki*error_int + Kd*d_error;
00214     }else{
00215         // Control output
00216         output = Kp*error + Ki*error_int;
00217     }
00218     //----------------------------------------//
00219 
00220     // Pre-integral action
00221     error_int_increment = Ts*error;
00222 
00223 }
00224 //--------------------------------------------------//
00225 
00226 
00227 // Used separately
00228 // Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- AntiWindUp(delta) -- output
00229 //////////////////////////////////////////////////
00230 void PID::Saturation_output(){
00231     //
00232     output = SAT_output.filter(output);
00233     delta_output = SAT_output.delta_out; // (original_out - limited_out)
00234 }
00235 void PID::AntiWindUp(float delta){ // delta_V = V - V_sat
00236 
00237     /*
00238     // (Analog) Anti-windup compensation
00239     // error_int -= Ka*delta; // Anti-windup
00240     error_int_increment -= Ka*delta; // Anti-windup
00241     */
00242 
00243     // (Digital) Anti-windup
00244     // If the output is going to be over bound, stop the integral action in that direction
00245     if (Ki > 0.0){
00246         //
00247         if (delta > overBound_value && error_int_increment > 0.0){ // Positive saturation
00248             error_int_increment = 0.0;
00249         }else if (delta < -overBound_value && error_int_increment < 0.0){ // Negative saturation
00250             error_int_increment = 0.0;
00251         }
00252     }else if (Ki < 0.0){
00253         //
00254         if (delta > overBound_value && error_int_increment < 0.0){ // Positive saturation
00255             error_int_increment = 0.0;
00256         }else if (delta < -overBound_value && error_int_increment > 0.0){ // Negative saturation
00257             error_int_increment = 0.0;
00258         }
00259     }
00260 
00261 }
00262 ////////////////////////////////////////////////// end Use separately
00263 
00264 // Used alone
00265 // Compute_noWindUP() -- Saturation_AntiWindUp() -- output
00266 //////////////////////////////////////////////////
00267 void PID::Saturation_AntiWindUp(){ // delta_V = V - V_sat
00268     //
00269     // Output saturation
00270     this->Saturation_output();
00271     // Anti-windup compensation
00272     this->AntiWindUp(delta_output);
00273 }
00274 ////////////////////////////////////////////////// end Use alone