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