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     setSetPoint(setPoint_);
00103 }
00104 
00105 void PID::setOutputLimits(float outMin, float outMax) {
00106 
00107     //Make sure we haven't been given impossible values.
00108     if (outMin >= outMax) {
00109         return;
00110     }
00111 
00112     //Rescale the working variables to reflect the changes.
00113     prevControllerOutput_ *= (outMax - outMin) / outSpan_;
00114 
00115     //Make sure the working variables are within the new limits.
00116     if (prevControllerOutput_ > 1) {
00117         prevControllerOutput_ = 1;
00118     } else if (prevControllerOutput_ < 0) {
00119         prevControllerOutput_ = 0;
00120     }
00121 
00122     outMin_  = outMin;
00123     outMax_  = outMax;
00124     outSpan_ = outMax - outMin;
00125 }
00126 
00127 /*void PID::setConInputLimits(float cinMin, float cinMax) {
00128     
00129     //Make sure we haven't been given impossible values.
00130     if ((cinMin >= cinMax) || (cinMin < inMin_) || (cinMax > inMax_)) {
00131         return;
00132     }
00133     
00134     cinSMin_ = (cinMin - inMin_)/inSpan_;
00135     cinSMax_ = (cinMax - inMax_)/inSpan_;
00136 }*/
00137 
00138 void PID::setTunings(float Kc, float tauI, float tauD) {
00139 
00140     //Verify that the tunings make sense.
00141     if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
00142         return;
00143     }
00144 
00145     //Store raw values to hand back to user on request.
00146     pParam_ = Kc;
00147     iParam_ = tauI;
00148     dParam_ = tauD;
00149 
00150     float tempTauR;
00151 
00152     if (tauI == 0.0) {
00153         tempTauR = 0.0;
00154     } else {
00155         tempTauR = (1.0 / tauI) * tSample_;
00156     }
00157 
00158     //For "bumpless transfer" we need to rescale the accumulated error.
00159     if (inAuto) {
00160         if (tempTauR == 0.0) {
00161             accError_ = 0.0;
00162         } else {
00163             accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
00164         }
00165     }
00166 
00167     Kc_   = Kc;
00168     tauR_ = tempTauR;
00169     tauD_ = tauD / tSample_;
00170 
00171 }
00172 
00173 void PID::reset(void) {
00174 
00175     float scaledBias = 0.0;
00176 
00177     if (usingFeedForward) {
00178         scaledBias = (bias_ - outMin_) / outSpan_;
00179     } else {
00180         scaledBias = (realOutput_ - outMin_) / outSpan_;
00181     }
00182 
00183     prevControllerOutput_ = scaledBias;
00184     prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
00185 
00186     //Clear any error in the integral.
00187     accError_ = 0;
00188 
00189 }
00190 
00191 void PID::setMode(int mode) {
00192 
00193     //We were in manual, and we just got set to auto.
00194     //Reset the controller internals.
00195     if (mode != 0 && !inAuto) {
00196         reset();
00197     }
00198 
00199     inAuto = (mode != 0);
00200 
00201 }
00202 
00203 void PID::setInterval(float interval) {
00204 
00205     if (interval > 0) {
00206         //Convert the time-based tunings to reflect this change.
00207         tauR_     *= (interval / tSample_);
00208         accError_ *= (tSample_ / interval);
00209         tauD_     *= (interval / tSample_);
00210         tSample_   = interval;
00211     }
00212 
00213 }
00214 
00215 void PID::setSetPoint(float sp) {
00216 
00217     if (sp > inMax_) {
00218         sp = inMax_;
00219     } else if (sp < inMin_) {
00220         sp = inMin_;
00221     }
00222     setPoint_ = sp;
00223 
00224 }
00225 
00226 void PID::setProcessValue(float pv) {
00227 
00228     processVariable_ = pv;
00229 
00230 }
00231 
00232 void PID::setBias(float bias){
00233 
00234     bias_ = bias;
00235     usingFeedForward = 1;
00236 
00237 }
00238 
00239 void PID::setAccLimit(float accLimit) {
00240     
00241     accLimit_ = abs(accLimit);
00242     
00243 }
00244     
00245 
00246 float PID::compute() {
00247 
00248     //Pull in the input and setpoint, and scale them into percent span.
00249     float scaledPV = (processVariable_ - inMin_) / inSpan_;
00250 
00251     /*if (scaledPV > 1.0) {
00252         scaledPV = 1.0;
00253     } else if (scaledPV < 0.0) {
00254         scaledPV = 0.0;
00255     }*/
00256 
00257     float scaledSP = (setPoint_ - inMin_) / inSpan_;
00258     /*if (scaledSP > 1.0) {
00259         scaledSP = 1.0;
00260     } else if (scaledSP < 0.0) {
00261         scaledSP = 0.0;
00262     }*/
00263 
00264     error_ = scaledSP - scaledPV;
00265 
00266     //Check and see if the output is pegged at a limit and only
00267     //integrate if it is not. This is to prevent reset-windup.
00268     if (!(prevControllerOutput_ >= 1 && error_ > 0) && !(prevControllerOutput_ <= 0 && error_ < 0)) {
00269         accError_ += error_;
00270         if (accError_ > accLimit_) {
00271             accError_ = accLimit_;
00272         } else if (accError_ < -accLimit_) {
00273             accError_ = -accLimit_;
00274         }
00275     }
00276 
00277     //Compute the current slope of the input signal.
00278     float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
00279 
00280     float scaledBias = 0.0;
00281 
00282     if (usingFeedForward) {
00283         scaledBias = (bias_ - outMin_) / outSpan_;
00284     }
00285 
00286     //Perform the PID calculation.
00287     controllerOutput_ = scaledBias + Kc_ * (error_ + (tauR_ * accError_) - (tauD_ * dMeas));
00288 
00289     //Make sure the computed output is within output constraints.
00290     if (controllerOutput_ < 0.0) {
00291         controllerOutput_ = 0.0;
00292     } else if (controllerOutput_ > 1.0) {
00293         controllerOutput_ = 1.0;
00294     }
00295 
00296     //Remember this output for the windup check next time.
00297     prevControllerOutput_ = controllerOutput_;
00298     //Remember the input for the derivative calculation next time.
00299     prevProcessVariable_  = scaledPV;
00300 
00301     //Scale the output from percent span back out to a real world number.
00302     return ((controllerOutput_ * outSpan_) + outMin_);
00303 
00304 }
00305 
00306 float PID::getInMin() {
00307     return inMin_;
00308 }
00309 
00310 float PID::getInMax() {
00311     return inMax_;
00312 }
00313 
00314 float PID::getOutMin() {
00315     return outMin_;
00316 }
00317 
00318 float PID::getOutMax() {
00319     return outMax_;
00320 }
00321 
00322 float PID::getInterval() {
00323     return tSample_;
00324 }
00325 
00326 float PID::getPParam() {
00327     return pParam_;
00328 }
00329 
00330 float PID::getIParam() {
00331     return iParam_;
00332 }
00333 
00334 float PID::getDParam() {
00335     return dParam_;
00336 }
00337 
00338 float PID::getAccLimit() {
00339     return accLimit_;
00340 }
00341 
00342 float PID::getAccError() {
00343     return accError_;
00344 }
00345 
00346 
00347 float PID::getSetPoint() {
00348     return setPoint_;
00349 }