Forked repository for PID library, I do not think I made any changes to this
Dependents: Axis Axis_20181108
Fork of PID by
PID.cpp
00001 /** 00002 * @author Aaron Berk 00003 * 00004 * @section LICENSE 00005 * 00006 * Copyright (c) 2010 ARM Limited 00007 * 00008 * Permission is hereby granted, free of charge, to any person obtaining a copy 00009 * of this software and associated documentation files (the "Software"), to deal 00010 * in the Software without restriction, including without limitation the rights 00011 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the Software is 00013 * furnished to do so, subject to the following conditions: 00014 * 00015 * The above copyright notice and this permission notice shall be included in 00016 * all copies or substantial portions of the Software. 00017 * 00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00019 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00020 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00021 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00022 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00023 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00024 * THE SOFTWARE. 00025 * 00026 * @section DESCRIPTION 00027 * 00028 * A PID controller is a widely used feedback controller commonly found in 00029 * industry. 00030 * 00031 * This library is a port of Brett Beauregard's Arduino PID library: 00032 * 00033 * http://www.arduino.cc/playground/Code/PIDLibrary 00034 * 00035 * The wikipedia article on PID controllers is a good place to start on 00036 * understanding how they work: 00037 * 00038 * http://en.wikipedia.org/wiki/PID_controller 00039 * 00040 * For a clear and elegant explanation of how to implement and tune a 00041 * controller, the controlguru website by Douglas J. Cooper (who also happened 00042 * to be Brett's controls professor) is an excellent reference: 00043 * 00044 * http://www.controlguru.com/ 00045 */ 00046 00047 /** 00048 * Includes 00049 */ 00050 #include "PID.h" 00051 00052 PID::PID(float Kc, float tauI, float tauD, float interval) { 00053 00054 usingFeedForward = false; 00055 inAuto = false; 00056 00057 //Default the limits to the full range of I/O: 3.3V 00058 //Make sure to set these to more appropriate limits for 00059 //your application. 00060 setInputLimits(0.0, 3.3); 00061 setOutputLimits(0.0, 3.3); 00062 00063 tSample_ = interval; 00064 00065 setTunings(Kc, tauI, tauD); 00066 00067 setPoint_ = 0.0; 00068 processVariable_ = 0.0; 00069 prevProcessVariable_ = 0.0; 00070 controllerOutput_ = 0.0; 00071 prevControllerOutput_ = 0.0; 00072 00073 accError_ = 0.0; 00074 bias_ = 0.0; 00075 00076 realOutput_ = 0.0; 00077 00078 } 00079 00080 void PID::setInputLimits(float inMin, float inMax) { 00081 00082 //Make sure we haven't been given impossible values. 00083 if (inMin >= inMax) { 00084 return; 00085 } 00086 00087 //Rescale the working variables to reflect the changes. 00088 prevProcessVariable_ *= (inMax - inMin) / inSpan_; 00089 accError_ *= (inMax - inMin) / inSpan_; 00090 00091 //Make sure the working variables are within the new limits. 00092 if (prevProcessVariable_ > 1) { 00093 prevProcessVariable_ = 1; 00094 } else if (prevProcessVariable_ < 0) { 00095 prevProcessVariable_ = 0; 00096 } 00097 00098 inMin_ = inMin; 00099 inMax_ = inMax; 00100 inSpan_ = inMax - inMin; 00101 00102 } 00103 00104 void PID::setOutputLimits(float outMin, float outMax) { 00105 00106 //Make sure we haven't been given impossible values. 00107 if (outMin >= outMax) { 00108 return; 00109 } 00110 00111 //Rescale the working variables to reflect the changes. 00112 prevControllerOutput_ *= (outMax - outMin) / outSpan_; 00113 00114 //Make sure the working variables are within the new limits. 00115 if (prevControllerOutput_ > 1) { 00116 prevControllerOutput_ = 1; 00117 } else if (prevControllerOutput_ < 0) { 00118 prevControllerOutput_ = 0; 00119 } 00120 00121 outMin_ = outMin; 00122 outMax_ = outMax; 00123 outSpan_ = outMax - outMin; 00124 00125 } 00126 00127 void PID::setTunings(float Kc, float tauI, float tauD) { 00128 00129 //Verify that the tunings make sense. 00130 /* if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) { 00131 return; 00132 } 00133 */ 00134 //Store raw values to hand back to user on request. 00135 pParam_ = Kc; 00136 iParam_ = tauI; 00137 dParam_ = tauD; 00138 00139 float tempTauR; 00140 00141 if (tauI == 0.0) { 00142 tempTauR = 0.0; 00143 } else { 00144 tempTauR = (1.0 / tauI) * tSample_; 00145 } 00146 00147 //For "bumpless transfer" we need to rescale the accumulated error. 00148 if (inAuto) { 00149 if (tempTauR == 0.0) { 00150 accError_ = 0.0; 00151 } else { 00152 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR); 00153 } 00154 } 00155 00156 Kc_ = Kc; 00157 tauR_ = tempTauR; 00158 tauD_ = tauD / tSample_; 00159 00160 } 00161 00162 void PID::reset(void) { 00163 00164 float scaledBias = 0.0; 00165 00166 if (usingFeedForward) { 00167 scaledBias = (bias_ - outMin_) / outSpan_; 00168 } else { 00169 scaledBias = (realOutput_ - outMin_) / outSpan_; 00170 } 00171 00172 prevControllerOutput_ = scaledBias; 00173 prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_; 00174 00175 //Clear any error in the integral. 00176 accError_ = 0; 00177 00178 } 00179 00180 void PID::setMode(int mode) { 00181 00182 //We were in manual, and we just got set to auto. 00183 //Reset the controller internals. 00184 if (mode != 0 && !inAuto) { 00185 reset(); 00186 } 00187 00188 inAuto = (mode != 0); 00189 00190 } 00191 00192 void PID::setInterval(float interval) { 00193 00194 if (interval > 0) { 00195 //Convert the time-based tunings to reflect this change. 00196 tauR_ *= (interval / tSample_); 00197 accError_ *= (tSample_ / interval); 00198 tauD_ *= (interval / tSample_); 00199 tSample_ = interval; 00200 } 00201 00202 } 00203 00204 void PID::setSetPoint(float sp) { 00205 00206 setPoint_ = sp; 00207 00208 } 00209 00210 void PID::setProcessValue(float pv) { 00211 00212 processVariable_ = pv; 00213 00214 } 00215 00216 void PID::setBias(float bias){ 00217 00218 bias_ = bias; 00219 usingFeedForward = 1; 00220 00221 } 00222 00223 float PID::compute() { 00224 00225 //Pull in the input and setpoint, and scale them into percent span. 00226 float scaledPV = (processVariable_ - inMin_) / inSpan_; 00227 00228 if (scaledPV > 1.0) { 00229 scaledPV = 1.0; 00230 } else if (scaledPV < 0.0) { 00231 scaledPV = 0.0; 00232 } 00233 00234 float scaledSP = (setPoint_ - inMin_) / inSpan_; 00235 if (scaledSP > 1.0) { 00236 scaledSP = 1; 00237 } else if (scaledSP < 0.0) { 00238 scaledSP = 0; 00239 } 00240 00241 float error = scaledSP - scaledPV; 00242 00243 //Check and see if the output is pegged at a limit and only 00244 //integrate if it is not. This is to prevent reset-windup. 00245 if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) { 00246 accError_ += error; 00247 } 00248 00249 //Compute the current slope of the input signal. 00250 float dMeas = (scaledPV - prevProcessVariable_) / tSample_; 00251 00252 float scaledBias = 0.0; 00253 00254 if (usingFeedForward) { 00255 scaledBias = (bias_ - outMin_) / outSpan_; 00256 } 00257 00258 //Perform the PID calculation. 00259 controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas)); 00260 00261 //Make sure the computed output is within output constraints. 00262 if (controllerOutput_ < 0.0) { 00263 controllerOutput_ = 0.0; 00264 } else if (controllerOutput_ > 1.0) { 00265 controllerOutput_ = 1.0; 00266 } 00267 00268 //Remember this output for the windup check next time. 00269 prevControllerOutput_ = controllerOutput_; 00270 //Remember the input for the derivative calculation next time. 00271 prevProcessVariable_ = scaledPV; 00272 00273 //Scale the output from percent span back out to a real world number. 00274 return ((controllerOutput_ * outSpan_) + outMin_); 00275 00276 } 00277 00278 float PID::getInMin() { 00279 00280 return inMin_; 00281 00282 } 00283 00284 float PID::getInMax() { 00285 00286 return inMax_; 00287 00288 } 00289 00290 float PID::getOutMin() { 00291 00292 return outMin_; 00293 00294 } 00295 00296 float PID::getOutMax() { 00297 00298 return outMax_; 00299 00300 } 00301 00302 float PID::getInterval() { 00303 00304 return tSample_; 00305 00306 } 00307 00308 float PID::getPParam() { 00309 00310 return pParam_; 00311 00312 } 00313 00314 float PID::getIParam() { 00315 00316 return iParam_; 00317 00318 } 00319 00320 float PID::getDParam() { 00321 00322 return dParam_; 00323 00324 }
Generated on Sat Jul 23 2022 08:40:45 by 1.7.2