Chun Feng Huang / PID

New PID library with digital anti-windup and process control

Fork of PID_modified by Chun Feng Huang

PID.cpp@7:6f0e5de35b48, 2017-03-30 (annotated)

Committer:
benson516
Date:
Thu Mar 30 07:52:35 2017 +0000
Revision:
7:6f0e5de35b48
Parent:
6:0d1e877c7f60 
New PID library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
weisnail 0:7f9b4ca968ae 1 #include "PID.h"
weisnail 0:7f9b4ca968ae 2
benson516 7:6f0e5de35b48 3 PID::PID(float Kp_in, float Ki_in, float Kd_in, float Sampletime_in):
benson516 7:6f0e5de35b48 4 derivative_error(Sampletime_in),
benson516 7:6f0e5de35b48 5 SAT_command(1,-1),
benson516 7:6f0e5de35b48 6 SAT_output(1,-1)
benson516 7:6f0e5de35b48 7 {
benson516 7:6f0e5de35b48 8 // To enble, run this->start() function
benson516 7:6f0e5de35b48 9 enable = false;
benson516 7:6f0e5de35b48 10
benson516 7:6f0e5de35b48 11 // Sampling time
benson516 7:6f0e5de35b48 12 Ts = Sampletime_in;
benson516 7:6f0e5de35b48 13
benson516 7:6f0e5de35b48 14
benson516 7:6f0e5de35b48 15 // Parameters
benson516 7:6f0e5de35b48 16 is_limiting_command = false;
benson516 7:6f0e5de35b48 17 is_limiting_output = false; // true
benson516 7:6f0e5de35b48 18 //
benson516 7:6f0e5de35b48 19 is_using_integral = false; // Determine if the integral control is going to be used.
benson516 7:6f0e5de35b48 20 is_using_derivative = false; // Determine if the derivative control is going to be used.
benson516 7:6f0e5de35b48 21 //
benson516 7:6f0e5de35b48 22 is_using_outSource_d_error = false; // Determine whether using the signal for d_error or using numerical derivative to derive d_error from error.
benson516 7:6f0e5de35b48 23 //
benson516 7:6f0e5de35b48 24 is_antiWindUp = false; // true
benson516 7:6f0e5de35b48 25
benson516 2:b9610a2d2ea0 26 // Parameters
benson516 7:6f0e5de35b48 27 // Feedback gain
benson516 7:6f0e5de35b48 28 Kp = Kp_in;
benson516 7:6f0e5de35b48 29 Ki = Ki_in;
benson516 7:6f0e5de35b48 30 Kd = Kd_in;
benson516 7:6f0e5de35b48 31 //
benson516 7:6f0e5de35b48 32 if (Ki_in == 0.0){
benson516 7:6f0e5de35b48 33 is_using_integral = false;
benson516 7:6f0e5de35b48 34 }else{
benson516 7:6f0e5de35b48 35 is_using_integral = true;
benson516 7:6f0e5de35b48 36 }
benson516 7:6f0e5de35b48 37 //
benson516 7:6f0e5de35b48 38 if (Kd_in == 0.0){
benson516 7:6f0e5de35b48 39 is_using_derivative = false;
benson516 7:6f0e5de35b48 40 }else{
benson516 7:6f0e5de35b48 41 is_using_derivative = true;
benson516 7:6f0e5de35b48 42 }
benson516 7:6f0e5de35b48 43 //
benson516 7:6f0e5de35b48 44 // Ka = 0.0017;// Volt.-sec./rad
benson516 7:6f0e5de35b48 45 //
benson516 7:6f0e5de35b48 46 // Small over-bound value for numerical stability
benson516 7:6f0e5de35b48 47 overBound_value = 0.01; // Small positive value, adjustable
benson516 7:6f0e5de35b48 48
benson516 7:6f0e5de35b48 49
benson516 7:6f0e5de35b48 50 // States
benson516 7:6f0e5de35b48 51 error = 0.0;
benson516 7:6f0e5de35b48 52 d_error = 0.0;
benson516 7:6f0e5de35b48 53 error_int = 0.0;
benson516 7:6f0e5de35b48 54 //
benson516 7:6f0e5de35b48 55 error_int_increment = 0.0;
benson516 7:6f0e5de35b48 56
benson516 7:6f0e5de35b48 57 // Input signal
benson516 7:6f0e5de35b48 58 command = 0.0;
benson516 7:6f0e5de35b48 59 feedbackValue = 0.0;
benson516 7:6f0e5de35b48 60 // Output signal
benson516 7:6f0e5de35b48 61 output = 0.0;
benson516 7:6f0e5de35b48 62 // Error by saturation
benson516 7:6f0e5de35b48 63 delta_output = 0.0; // Error by saturation
benson516 7:6f0e5de35b48 64
benson516 7:6f0e5de35b48 65 }
benson516 7:6f0e5de35b48 66 // Process controller
benson516 7:6f0e5de35b48 67 void PID::start(){ // Run
benson516 7:6f0e5de35b48 68 if (enable){
benson516 7:6f0e5de35b48 69 return;
benson516 7:6f0e5de35b48 70 }
benson516 7:6f0e5de35b48 71 //
benson516 7:6f0e5de35b48 72 enable = true;
benson516 7:6f0e5de35b48 73 }
benson516 7:6f0e5de35b48 74 void PID::pause(){ // Stop updating but no reset
benson516 7:6f0e5de35b48 75 //
benson516 7:6f0e5de35b48 76 enable = false;
benson516 7:6f0e5de35b48 77 }
benson516 7:6f0e5de35b48 78 void PID::stop(){ // Stop and reset
benson516 7:6f0e5de35b48 79 if (!enable){
benson516 7:6f0e5de35b48 80 return;
benson516 7:6f0e5de35b48 81 }
benson516 7:6f0e5de35b48 82 //
benson516 7:6f0e5de35b48 83 enable = false;
benson516 7:6f0e5de35b48 84 reset();
benson516 7:6f0e5de35b48 85 }
benson516 7:6f0e5de35b48 86 void PID::reset(void){
benson516 7:6f0e5de35b48 87 // States
benson516 7:6f0e5de35b48 88 error = 0.0;
benson516 7:6f0e5de35b48 89 d_error = 0.0;
benson516 7:6f0e5de35b48 90 error_int = 0.0;
benson516 7:6f0e5de35b48 91 //
benson516 7:6f0e5de35b48 92 error_int_increment = 0.0;
benson516 7:6f0e5de35b48 93
benson516 7:6f0e5de35b48 94 // Input signal
benson516 7:6f0e5de35b48 95 command = 0.0;
benson516 7:6f0e5de35b48 96 feedbackValue = 0.0;
benson516 7:6f0e5de35b48 97 // Output signal
benson516 7:6f0e5de35b48 98 output = 0.0;
benson516 7:6f0e5de35b48 99 // Error by saturation
benson516 7:6f0e5de35b48 100 delta_output = 0.0; // Error by saturation
benson516 7:6f0e5de35b48 101
benson516 7:6f0e5de35b48 102 // Reset the derivative
benson516 7:6f0e5de35b48 103 derivative_error.reset(0.0);
benson516 7:6f0e5de35b48 104 }
benson516 7:6f0e5de35b48 105 //
benson516 7:6f0e5de35b48 106 void PID::EnableAntiWindUp(float Ka_in)
benson516 7:6f0e5de35b48 107 {
benson516 7:6f0e5de35b48 108 is_antiWindUp = true;
benson516 7:6f0e5de35b48 109 }
benson516 7:6f0e5de35b48 110 //
benson516 7:6f0e5de35b48 111 void PID::set_PID_gains(float Kp_in, float Ki_in, float Kd_in){ // Setting Kp, Ki, and Kd
benson516 2:b9610a2d2ea0 112 Kp = Kp_in;
benson516 2:b9610a2d2ea0 113 Ki = Ki_in;
benson516 2:b9610a2d2ea0 114 Kd = Kd_in;
benson516 2:b9610a2d2ea0 115 //
benson516 7:6f0e5de35b48 116 if (Ki_in == 0.0){
benson516 7:6f0e5de35b48 117 is_using_integral = false;
benson516 7:6f0e5de35b48 118 }else{
benson516 7:6f0e5de35b48 119 is_using_integral = true;
benson516 7:6f0e5de35b48 120 }
benson516 7:6f0e5de35b48 121 //
benson516 7:6f0e5de35b48 122 if (Kd_in == 0.0){
benson516 7:6f0e5de35b48 123 is_using_derivative = false;
benson516 7:6f0e5de35b48 124 }else{
benson516 7:6f0e5de35b48 125 is_using_derivative = true;
benson516 7:6f0e5de35b48 126 }
weisnail 0:7f9b4ca968ae 127 }
benson516 7:6f0e5de35b48 128 void PID::SetInputLimits(float inputLimits_H_in, float inputLimits_L_in){
benson516 7:6f0e5de35b48 129 is_limiting_command = true;
benson516 7:6f0e5de35b48 130 //
benson516 7:6f0e5de35b48 131 SAT_command.set_bound(inputLimits_H_in, inputLimits_L_in);
weisnail 0:7f9b4ca968ae 132 }
benson516 7:6f0e5de35b48 133 void PID::SetOutputLimits(float outputLimits_H_in, float outputLimits_L_in){
benson516 7:6f0e5de35b48 134 is_limiting_output = true;
benson516 7:6f0e5de35b48 135 // is_antiWindUp = true;
benson516 7:6f0e5de35b48 136 //
benson516 7:6f0e5de35b48 137 SAT_output.set_bound(outputLimits_H_in, outputLimits_L_in);
weisnail 0:7f9b4ca968ae 138
benson516 7:6f0e5de35b48 139 //
benson516 7:6f0e5de35b48 140 // Set the over-bound value to be 1% of peak-peak range
benson516 7:6f0e5de35b48 141 overBound_value = 0.001*(outputLimits_H_in - outputLimits_L_in);
adam_z 1:4df4895863cd 142 }
adam_z 1:4df4895863cd 143
benson516 7:6f0e5de35b48 144 // Main function for computing the PID
benson516 7:6f0e5de35b48 145 //--------------------------------------------------//
benson516 7:6f0e5de35b48 146 void PID::set_d_error(float d_error_in){ // Insert d_error before iteration.
benson516 7:6f0e5de35b48 147 d_error = d_error_in;
benson516 7:6f0e5de35b48 148 is_using_outSource_d_error = true;
benson516 7:6f0e5de35b48 149 }
benson516 7:6f0e5de35b48 150 //
benson516 7:6f0e5de35b48 151 void PID::iterateOnce(float command_in, float feedbackValue_in){
benson516 7:6f0e5de35b48 152 // Main process
benson516 7:6f0e5de35b48 153 iterateOnce_noAntiWindUP(command_in, feedbackValue_in);
weisnail 0:7f9b4ca968ae 154
benson516 7:6f0e5de35b48 155 // Output satuation
benson516 7:6f0e5de35b48 156 if(is_limiting_output){
benson516 7:6f0e5de35b48 157 if (is_antiWindUp){
benson516 7:6f0e5de35b48 158 // Output saturation + anti-windup
benson516 7:6f0e5de35b48 159 this->Saturation_AntiWindUp();
benson516 2:b9610a2d2ea0 160 }else{
benson516 7:6f0e5de35b48 161 // Output saturation only
benson516 7:6f0e5de35b48 162 this->Saturation_output();
weisnail 0:7f9b4ca968ae 163 }
benson516 7:6f0e5de35b48 164 //
weisnail 0:7f9b4ca968ae 165 }else{
benson516 2:b9610a2d2ea0 166 // output = output;
weisnail 0:7f9b4ca968ae 167 }
benson516 7:6f0e5de35b48 168
adam_z 1:4df4895863cd 169 }
benson516 7:6f0e5de35b48 170 //
benson516 7:6f0e5de35b48 171 void PID::iterateOnce_noAntiWindUP(float command_in, float feedbackValue_in){
adam_z 1:4df4895863cd 172
benson516 7:6f0e5de35b48 173 //
benson516 7:6f0e5de35b48 174 // -- Important! --
benson516 7:6f0e5de35b48 175 // Post-integral action:
benson516 7:6f0e5de35b48 176 // This integral action generates the error_int of time (k-1) (previous time), which means the back-step integral.
benson516 7:6f0e5de35b48 177 // The actual integral action move to here is for implementing the (digital-version) anti-windup.
benson516 7:6f0e5de35b48 178 if (is_using_integral){
benson516 7:6f0e5de35b48 179 error_int += error_int_increment;
benson516 4:e3c9cb64be44 180 }else{
benson516 7:6f0e5de35b48 181 error_int = 0.0;
benson516 7:6f0e5de35b48 182 }
benson516 7:6f0e5de35b48 183 //
benson516 7:6f0e5de35b48 184
benson516 7:6f0e5de35b48 185 // Processing input signals
benson516 7:6f0e5de35b48 186 //----------------------------------------//
benson516 7:6f0e5de35b48 187 // Comand saturation
benson516 7:6f0e5de35b48 188 if(is_limiting_command){
benson516 7:6f0e5de35b48 189 // Saturation
benson516 7:6f0e5de35b48 190 command = SAT_command.filter(command_in);
benson516 7:6f0e5de35b48 191 //
benson516 7:6f0e5de35b48 192 }else{
benson516 7:6f0e5de35b48 193 command = command_in;
benson516 4:e3c9cb64be44 194 }
benson516 7:6f0e5de35b48 195
benson516 7:6f0e5de35b48 196 // bypass the feedback value
benson516 7:6f0e5de35b48 197 feedbackValue = feedbackValue_in;
benson516 7:6f0e5de35b48 198 //----------------------------------------//
benson516 7:6f0e5de35b48 199
benson516 7:6f0e5de35b48 200 // PID control
benson516 7:6f0e5de35b48 201 //----------------------------------------//
benson516 7:6f0e5de35b48 202 // Calculating the error
benson516 7:6f0e5de35b48 203 error = command - feedbackValue;
benson516 7:6f0e5de35b48 204 //
benson516 7:6f0e5de35b48 205 if (is_using_derivative){
benson516 7:6f0e5de35b48 206 if (is_using_outSource_d_error){
benson516 7:6f0e5de35b48 207 // Please use the insert function for d_error.
benson516 7:6f0e5de35b48 208 }else{
benson516 7:6f0e5de35b48 209 // Calculating the derivative of error
benson516 7:6f0e5de35b48 210 d_error = derivative_error.filter(error);
benson516 4:e3c9cb64be44 211 }
benson516 7:6f0e5de35b48 212 // Control output
benson516 7:6f0e5de35b48 213 output = Kp*error + Ki*error_int + Kd*d_error;
benson516 4:e3c9cb64be44 214 }else{
benson516 7:6f0e5de35b48 215 // Control output
benson516 7:6f0e5de35b48 216 output = Kp*error + Ki*error_int;
benson516 4:e3c9cb64be44 217 }
benson516 7:6f0e5de35b48 218 //----------------------------------------//
benson516 7:6f0e5de35b48 219
benson516 7:6f0e5de35b48 220 // Pre-integral action
benson516 7:6f0e5de35b48 221 error_int_increment = Ts*error;
benson516 7:6f0e5de35b48 222
benson516 4:e3c9cb64be44 223 }
benson516 7:6f0e5de35b48 224 //--------------------------------------------------//
benson516 5:016c99bb877f 225
benson516 7:6f0e5de35b48 226
benson516 7:6f0e5de35b48 227 // Used separately
benson516 7:6f0e5de35b48 228 // Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- AntiWindUp(delta) -- output
benson516 5:016c99bb877f 229 //////////////////////////////////////////////////
benson516 4:e3c9cb64be44 230 void PID::Saturation_output(){
benson516 7:6f0e5de35b48 231 //
benson516 7:6f0e5de35b48 232 output = SAT_output.filter(output);
benson516 7:6f0e5de35b48 233 delta_output = SAT_output.delta_out; // (original_out - limited_out)
benson516 7:6f0e5de35b48 234 }
benson516 7:6f0e5de35b48 235 void PID::AntiWindUp(float delta){ // delta_V = V - V_sat
benson516 7:6f0e5de35b48 236
benson516 7:6f0e5de35b48 237 /*
benson516 7:6f0e5de35b48 238 // (Analog) Anti-windup compensation
benson516 7:6f0e5de35b48 239 // error_int -= Ka*delta; // Anti-windup
benson516 7:6f0e5de35b48 240 error_int_increment -= Ka*delta; // Anti-windup
benson516 7:6f0e5de35b48 241 */
benson516 7:6f0e5de35b48 242
benson516 7:6f0e5de35b48 243 // (Digital) Anti-windup
benson516 7:6f0e5de35b48 244 // If the output is going to be over bound, stop the integral action in that direction
benson516 7:6f0e5de35b48 245 if (Ki > 0.0){
benson516 7:6f0e5de35b48 246 //
benson516 7:6f0e5de35b48 247 if (delta > overBound_value && error_int_increment > 0.0){ // Positive saturation
benson516 7:6f0e5de35b48 248 error_int_increment = 0.0;
benson516 7:6f0e5de35b48 249 }else if (delta < -overBound_value && error_int_increment < 0.0){ // Negative saturation
benson516 7:6f0e5de35b48 250 error_int_increment = 0.0;
benson516 4:e3c9cb64be44 251 }
benson516 7:6f0e5de35b48 252 }else if (Ki < 0.0){
benson516 7:6f0e5de35b48 253 //
benson516 7:6f0e5de35b48 254 if (delta > overBound_value && error_int_increment < 0.0){ // Positive saturation
benson516 7:6f0e5de35b48 255 error_int_increment = 0.0;
benson516 7:6f0e5de35b48 256 }else if (delta < -overBound_value && error_int_increment > 0.0){ // Negative saturation
benson516 7:6f0e5de35b48 257 error_int_increment = 0.0;
benson516 7:6f0e5de35b48 258 }
benson516 7:6f0e5de35b48 259 }
benson516 7:6f0e5de35b48 260
benson516 5:016c99bb877f 261 }
benson516 5:016c99bb877f 262 ////////////////////////////////////////////////// end Use separately
benson516 5:016c99bb877f 263
benson516 7:6f0e5de35b48 264 // Used alone
benson516 7:6f0e5de35b48 265 // Compute_noWindUP() -- Saturation_AntiWindUp() -- output
benson516 5:016c99bb877f 266 //////////////////////////////////////////////////
benson516 7:6f0e5de35b48 267 void PID::Saturation_AntiWindUp(){ // delta_V = V - V_sat
benson516 7:6f0e5de35b48 268 //
benson516 7:6f0e5de35b48 269 // Output saturation
benson516 7:6f0e5de35b48 270 this->Saturation_output();
benson516 5:016c99bb877f 271 // Anti-windup compensation
benson516 7:6f0e5de35b48 272 this->AntiWindUp(delta_output);
benson516 5:016c99bb877f 273 }
benson516 7:6f0e5de35b48 274 ////////////////////////////////////////////////// end Use alone