PID.cpp

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