PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float Kc, float tauI, float tauD, float interval) {
00004 
00005     usingFeedForward = false;
00006     inAuto           = false;
00007 
00008     setInputLimits(-90.0,90.0 );
00009     setOutputLimits(-3500.0, 3500.0);
00010 
00011     tSample_ = interval;
00012 
00013     setTunings(Kc, tauI, tauD);
00014 
00015     setPoint_             = 0.0;
00016     processVariable_      = 0.0;
00017     prevProcessVariable_  = 0.0;
00018     controllerOutput_     = 0.0;
00019     prevControllerOutput_ = 0.0;
00020 
00021     accError_ = 0.0;
00022     bias_     = 0.0;
00023     
00024     realOutput_ = 0.0;
00025 }
00026 
00027 void PID::setInputLimits(float inMin, float inMax) {
00028 
00029     //Make sure we haven't been given impossible values.
00030     if (inMin >= inMax) {
00031         return;
00032     }
00033 
00034     //Rescale the working variables to reflect the changes.
00035     prevProcessVariable_ *= (inMax - inMin) / inSpan_;
00036     accError_            *= (inMax - inMin) / inSpan_;
00037 
00038     //Make sure the working variables are within the new limits.
00039     if (prevProcessVariable_ > 1) {
00040         prevProcessVariable_ = 1;
00041     } else if (prevProcessVariable_ < 0) {
00042         prevProcessVariable_ = 0;
00043     }
00044 
00045     inMin_  = inMin;
00046     inMax_  = inMax;
00047     inSpan_ = inMax - inMin;
00048 
00049 }
00050 
00051 void PID::setOutputLimits(float outMin, float outMax) {
00052 
00053     //Make sure we haven't been given impossible values.
00054     if (outMin >= outMax) {
00055         return;
00056     }
00057 
00058     //Rescale the working variables to reflect the changes.
00059     prevControllerOutput_ *= (outMax - outMin) / outSpan_;
00060 
00061     //Make sure the working variables are within the new limits.
00062     if (prevControllerOutput_ > 1) {
00063         prevControllerOutput_ = 1;
00064     } else if (prevControllerOutput_ < 0) {
00065         prevControllerOutput_ = 0;
00066     }
00067 
00068     outMin_  = outMin;
00069     outMax_  = outMax;
00070     outSpan_ = outMax - outMin;
00071 
00072 }
00073 
00074 void PID::setTunings(float Kc, float tauI, float tauD) {
00075 
00076     //Verify that the tunings make sense.
00077     if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
00078         return;
00079     }
00080 
00081     //Store raw values to hand back to user on request.
00082     pParam_ = Kc;
00083     iParam_ = tauI;
00084     dParam_ = tauD;
00085 
00086     float tempTauR;
00087 
00088     if (tauI == 0.0) {
00089         tempTauR = 0.0;
00090     } else {
00091         tempTauR = (1.0 / tauI) * tSample_;
00092     }
00093 
00094     //For "bumpless transfer" we need to rescale the accumulated error.
00095     if (inAuto) {
00096         if (tempTauR == 0.0) {
00097             accError_ = 0.0;
00098         } else {
00099             accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
00100         }
00101     }
00102 
00103     Kc_   = Kc;
00104     tauR_ = tempTauR;
00105     tauD_ = tauD / tSample_;
00106 
00107 }
00108 
00109 void PID::reset(void) {
00110 
00111     float scaledBias = 0.0;
00112 
00113     if (usingFeedForward) {
00114         scaledBias = (bias_ - outMin_) / outSpan_;
00115     } else {
00116         scaledBias = (realOutput_ - outMin_) / outSpan_;
00117     }
00118 
00119     prevControllerOutput_ = scaledBias;
00120     prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
00121 
00122     //Clear any error in the integral.
00123     accError_ = 0;
00124 
00125 }
00126 
00127 void PID::setMode(int mode) {
00128 
00129     //We were in manual, and we just got set to auto.
00130     //Reset the controller internals.
00131     if (mode != 0 && !inAuto) {
00132         reset();
00133     }
00134 
00135     inAuto = (mode != 0);
00136 
00137 }
00138 
00139 void PID::setInterval(float interval) {
00140 
00141     if (interval > 0) {
00142         //Convert the time-based tunings to reflect this change.
00143         tauR_     *= (interval / tSample_);
00144         accError_ *= (tSample_ / interval);
00145         tauD_     *= (interval / tSample_);
00146         tSample_   = interval;
00147     }
00148 
00149 }
00150 
00151 void PID::setSetPoint(float sp) {
00152 
00153     setPoint_ = sp;
00154 
00155 }
00156 
00157 void PID::setProcessValue(float pv) {
00158 
00159     processVariable_ = pv;
00160 
00161 }
00162 
00163 void PID::setBias(float bias){
00164 
00165     bias_ = bias;
00166     usingFeedForward = 1;
00167 
00168 }
00169 
00170 float PID::compute() {
00171 
00172     //Pull in the input and setpoint, and scale them into percent span.
00173     float scaledPV = (processVariable_ - inMin_) / inSpan_;
00174 
00175     if (scaledPV > 1.0) {
00176         scaledPV = 1.0;
00177     } else if (scaledPV < 0.0) {
00178         scaledPV = 0.0;
00179     }
00180 
00181     float scaledSP = (setPoint_ - inMin_) / inSpan_;
00182     if (scaledSP > 1.0) {
00183         scaledSP = 1;
00184     } else if (scaledSP < 0.0) {
00185         scaledSP = 0;
00186     }
00187 
00188     float error = scaledSP - scaledPV;
00189 
00190     //Check and see if the output is pegged at a limit and only
00191     //integrate if it is not. This is to prevent reset-windup.
00192     if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
00193         accError_ += error;
00194     }
00195 
00196     //Compute the current slope of the input signal.
00197     float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
00198 
00199     float scaledBias = 0.0;
00200 
00201     if (usingFeedForward) {
00202         scaledBias = (bias_ - outMin_) / outSpan_;
00203     }
00204 
00205     //Perform the PID calculation.
00206     controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
00207 
00208     //Make sure the computed output is within output constraints.
00209     if (controllerOutput_ < 0.0) {
00210         controllerOutput_ = 0.0;
00211     } else if (controllerOutput_ > 1.0) {
00212         controllerOutput_ = 1.0;
00213     }
00214 
00215     //Remember this output for the windup check next time.
00216     prevControllerOutput_ = controllerOutput_;
00217     //Remember the input for the derivative calculation next time.
00218     prevProcessVariable_  = scaledPV;
00219 
00220     //Scale the output from percent span back out to a real world number.
00221     return ((controllerOutput_ * outSpan_) + outMin_);
00222 
00223 }
00224 
00225 float PID::getInMin() {
00226 
00227     return inMin_;
00228 
00229 }
00230 
00231 float PID::getInMax() {
00232 
00233     return inMax_;
00234 
00235 }
00236 
00237 float PID::getOutMin() {
00238 
00239     return outMin_;
00240 
00241 }
00242 
00243 float PID::getOutMax() {
00244 
00245     return outMax_;
00246 
00247 }
00248 
00249 float PID::getInterval() {
00250 
00251     return tSample_;
00252 
00253 }
00254 
00255 float PID::getPParam() {
00256 
00257     return pParam_;
00258 
00259 }
00260 
00261 float PID::getIParam() {
00262 
00263     return iParam_;
00264 
00265 }
00266 
00267 float PID::getDParam() {
00268 
00269     return dParam_;
00270 
00271 }