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 // Modified by K.Arai/JH1PJL on June 15th, 2016
00048 
00049 /**
00050  * Includes
00051  */
00052 #include "PID.h"
00053 
00054 PID::PID(double Kc, double tauI, double tauD, double interval) {
00055 
00056     usingFeedForward = false;
00057     inAuto           = false;
00058 
00059     //Default the limits to the full range of I/O: 3.3V
00060     //Make sure to set these to more appropriate limits for
00061     //your application.
00062     setInputLimits(0.0, 3.3);
00063     setOutputLimits(0.0, 3.3);
00064 
00065     tSample_ = interval;
00066 
00067     setTunings(Kc, tauI, tauD);
00068 
00069     setPoint_             = 0.0;
00070     processVariable_      = 0.0;
00071     prevProcessVariable_  = 0.0;
00072     controllerOutput_     = 0.0;
00073     prevControllerOutput_ = 0.0;
00074 
00075     accError_ = 0.0;
00076     bias_     = 0.0;
00077     
00078     realOutput_ = 0.0;
00079 
00080 }
00081 
00082 void PID::setInputLimits(double inMin, double inMax) {
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(double outMin, double outMax) {
00107 
00108     //Make sure we haven't been given impossible values.
00109     if (outMin >= outMax) {
00110         return;
00111     }
00112 
00113     //Rescale the working variables to reflect the changes.
00114     prevControllerOutput_ *= (outMax - outMin) / outSpan_;
00115 
00116     //Make sure the working variables are within the new limits.
00117     if (prevControllerOutput_ > 1) {
00118         prevControllerOutput_ = 1;
00119     } else if (prevControllerOutput_ < 0) {
00120         prevControllerOutput_ = 0;
00121     }
00122 
00123     outMin_  = outMin;
00124     outMax_  = outMax;
00125     outSpan_ = outMax - outMin;
00126 
00127 }
00128 
00129 void PID::setTunings(double Kc, double tauI, double 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     double 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     double 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(double 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(double sp) {
00207 
00208     setPoint_ = sp;
00209 
00210 }
00211 
00212 void PID::setProcessValue(double pv) {
00213 
00214     processVariable_ = pv;
00215 
00216 }
00217 
00218 void PID::setBias(double bias){
00219 
00220     bias_ = bias;
00221     usingFeedForward = 1;
00222 
00223 }
00224 
00225 double PID::compute() {
00226 
00227     //Pull in the input and setpoint, and scale them into percent span.
00228     double scaledPV = (processVariable_ - inMin_) / inSpan_;
00229 
00230     if (scaledPV > 1.0) {
00231         scaledPV = 1.0;
00232     } else if (scaledPV < 0.0) {
00233         scaledPV = 0.0;
00234     }
00235 
00236     double scaledSP = (setPoint_ - inMin_) / inSpan_;
00237     if (scaledSP > 1.0) {
00238         scaledSP = 1;
00239     } else if (scaledSP < 0.0) {
00240         scaledSP = 0;
00241     }
00242 
00243     double 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_ <= 0 && error < 0)) {
00248         accError_ += error;
00249     }
00250 
00251     //Compute the current slope of the input signal.
00252     double dMeas = (scaledPV - prevProcessVariable_) / tSample_;
00253 
00254     double scaledBias = 0.0;
00255 
00256     if (usingFeedForward) {
00257         scaledBias = (bias_ - outMin_) / outSpan_;
00258     }
00259 
00260     //Perform the PID calculation.
00261     controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
00262 
00263     //Make sure the computed output is within output constraints.
00264     if (controllerOutput_ < 0.0) {
00265         controllerOutput_ = 0.0;
00266     } else if (controllerOutput_ > 1.0) {
00267         controllerOutput_ = 1.0;
00268     }
00269 
00270     //Remember this output for the windup check next time.
00271     prevControllerOutput_ = controllerOutput_;
00272     //Remember the input for the derivative calculation next time.
00273     prevProcessVariable_  = scaledPV;
00274 
00275     //Scale the output from percent span back out to a real world number.
00276     return ((controllerOutput_ * outSpan_) + outMin_);
00277 
00278 }
00279 
00280 double PID::getInMin() {
00281 
00282     return inMin_;
00283 
00284 }
00285 
00286 double PID::getInMax() {
00287 
00288     return inMax_;
00289 
00290 }
00291 
00292 double PID::getOutMin() {
00293 
00294     return outMin_;
00295 
00296 }
00297 
00298 double PID::getOutMax() {
00299 
00300     return outMax_;
00301 
00302 }
00303 
00304 double PID::getInterval() {
00305 
00306     return tSample_;
00307 
00308 }
00309 
00310 double PID::getPParam() {
00311 
00312     return pParam_;
00313 
00314 }
00315 
00316 double PID::getIParam() {
00317 
00318     return iParam_;
00319 
00320 }
00321 
00322 double PID::getDParam() {
00323 
00324     return dParam_;
00325 
00326 }