controller.cpp

00001 #include "mbed.h"
00002 #include "controller.h"
00003 
00004 
00005 
00006 
00007 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
00008  
00009  //BX tune
00010     setInputLimits(in_min,in_max);
00011     setOutputLimits(out_min,out_max);
00012  
00013     tSample_ = interval;
00014  
00015     setTunings(Kc, tauI, tauD);
00016  
00017     setPoint_ = 0.0;
00018     processVariable_ = 0.0;
00019     prevProcessVariable_ = 0.0;
00020     controllerOutput_ = 0.0;
00021     prevControllerOutput_ = 0.0;
00022  
00023     
00024  
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    
00035  
00036    
00037  
00038     inMin_ = inMin;
00039     inMax_ = inMax;
00040     inSpan_ = (inMax - inMin);
00041  
00042  
00043  
00044  
00045 }
00046  
00047 void PID::setOutputLimits(float outMin, float outMax) {
00048  
00049     //Make sure we haven't been given impossible values.
00050     if (outMin >= outMax) {
00051         return;
00052     }
00053  
00054     
00055  
00056    //ppp
00057  
00058     outMin_ = outMin;
00059     outMax_ = outMax;
00060     outMid_ = (outMin+outMax)/2;
00061     
00062     outSpan_ = (outMax - outMin);
00063    
00064     
00065     
00066     
00067     
00068  
00069 }
00070  
00071  
00072  
00073 //-------------------------------------------------- 
00074  
00075  
00076  
00077 void PID::setTunings(float Kc, float tauI, float tauD) {
00078  
00079     //Verify that the tunings make sense.
00080     if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
00081         return;
00082     }
00083  
00084     //Store raw values to hand back to user on request.
00085     pParam_ = Kc;
00086     iParam_ = tauI;
00087     dParam_ = tauD;
00088  
00089    
00090  
00091   
00092  
00093     Kc_ = Kc;
00094     tauI_=tauI;
00095     tauD_=tauD;
00096  
00097 }
00098 
00099 
00100 float PID::compute(float pv,  float sp) {
00101  
00102     
00103     processVariable_ = pv; //ce que l'on mesure
00104     setPoint_ = sp;  // ce que l'on veut atteindre
00105  
00106  
00107  
00108  
00109  
00110  
00111     //Pull in the input and setpoint, and scale them into percent span.
00112     float scaledPV = (processVariable_ - inMin_) / inSpan_;
00113  
00114     if (scaledPV > 1.0) {
00115         scaledPV = 1.0;
00116     }
00117     else if (scaledPV < 0.0) {
00118         scaledPV = 0.0;
00119     }
00120  
00121     float scaledSP = (setPoint_ - inMin_) / inSpan_;
00122     if (scaledSP > 1.0) {
00123         scaledSP = 1;
00124     }
00125     else if (scaledSP < 0.0) {
00126         scaledSP = 0;
00127     }
00128  
00129     float error = (scaledSP - scaledPV)*2;
00130 // 100~ -100%
00131     
00132 
00133 
00134 
00135 // if error add
00136  if ( (-1<error && error <1)             ) {
00137         accError_ = (accError_*0.66)+ error;
00138     }
00139 
00140      float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
00141   
00142      controllerOutput_= Kc_* error+(tauI_*accError_)+tauD_*dMeas+outMid_;
00143      de_ip=accError_;
00144      de_dp=dMeas;
00145      de_kp=error;
00146    
00147    
00148    
00149    
00150      
00151         
00152       if (controllerOutput_ < outMin_) {
00153         controllerOutput_ = outMin_;
00154     }
00155     else if (controllerOutput_ > outMax_) {
00156         controllerOutput_ = outMax_;
00157     }
00158    
00159    
00160    
00161    
00162    prevProcessVariable_ = scaledPV;
00163     
00164      
00165     return (controllerOutput_);
00166  
00167 }
00168  
00169 float PID::getInMin() {
00170  
00171     return inMin_;
00172  
00173 }
00174  
00175 float PID::getInMax() {
00176  
00177     return inMax_;
00178  
00179 }
00180  
00181 float PID::getOutMin() {
00182  
00183     return outMin_;
00184  
00185 }
00186  
00187 float PID::getOutMax() {
00188  
00189     return outMax_;
00190  
00191 }
00192  
00193 float PID::getInterval() {
00194  
00195     return tSample_;
00196  
00197 }
00198  
00199 float PID::getPParam() {
00200  
00201     return pParam_;
00202  
00203 }
00204  
00205 float PID::getIParam() {
00206  
00207     return iParam_;
00208  
00209 }
00210  
00211 float PID::getDParam() {
00212  
00213     return dParam_;
00214  
00215 }
00216