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