New PID library with digital anti-windup and process control
Fork of PID_modified by
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
Generated on Wed Jul 13 2022 12:28:10 by
1.7.2
