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_ < 1.0) {
00095         prevProcessVariable_ = -1.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_ < -1.0) {
00118         prevControllerOutput_ = -1.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     float scaledPV = processVariable_;
00228 
00229     if (scaledPV > 1.0) {
00230         scaledPV = 1.0;
00231     } else if (scaledPV < -1.0) {
00232         scaledPV = -1.0;
00233     }
00234 
00235     //float scaledSP = (setPoint_ - inMin_) / inSpan_;
00236     float scaledSP = setPoint_;
00237     if (scaledSP > 1.0) {
00238         scaledSP = 1;
00239     } else if (scaledSP < -1.0) {
00240         scaledSP = -1.0;
00241     }
00242 
00243     float error = scaledSP - scaledPV;
00244 
00245     //Check and see if the output is pegged at a limit and only
00246     //integrate if it is not. This is to prevent reset-windup.
00247     if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= -1.0 && error < 0)) {
00248         accError_ += error;
00249     }
00250 
00251     //Compute the current slope of the input signal.
00252     float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
00253 
00254     float scaledBias = 0.0;
00255 
00256     if (usingFeedForward) {
00257         //scaledBias = (bias_ - outMin_) / outSpan_;
00258         scaledBias = bias_;
00259     }
00260 
00261     //Perform the PID calculation.
00262     controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
00263 
00264     //Make sure the computed output is within output constraints.
00265     if (controllerOutput_ < -1.0) {
00266         controllerOutput_ = -1.0;
00267     } else if (controllerOutput_ > 1.0) {
00268         controllerOutput_ = 1.0;
00269     }
00270 
00271     //Remember this output for the windup check next time.
00272     prevControllerOutput_ = controllerOutput_;
00273     //Remember the input for the derivative calculation next time.
00274     prevProcessVariable_  = scaledPV;
00275 
00276     //Scale the output from percent span back out to a real world number.
00277     //return (controllerOutput_ * outSpan_);
00278     return controllerOutput_;
00279 
00280 }
00281 
00282 float PID::getInMin() {
00283 
00284     return inMin_;
00285 
00286 }
00287 
00288 float PID::getInMax() {
00289 
00290     return inMax_;
00291 
00292 }
00293 
00294 float PID::getOutMin() {
00295 
00296     return outMin_;
00297 
00298 }
00299 
00300 float PID::getOutMax() {
00301 
00302     return outMax_;
00303 
00304 }
00305 
00306 float PID::getInterval() {
00307 
00308     return tSample_;
00309 
00310 }
00311 
00312 float PID::getPParam() {
00313 
00314     return pParam_;
00315 
00316 }
00317 
00318 float PID::getIParam() {
00319 
00320     return iParam_;
00321 
00322 }
00323 
00324 float PID::getDParam() {
00325 
00326     return dParam_;
00327 
00328 }