File content as of revision 17:3dac99cf2b89:

#include "mbed.h"
#include "controller.h"




PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
 
 //BX tune
    setInputLimits(in_min,in_max);
    setOutputLimits(out_min,out_max);
 
    tSample_ = interval;
 
    setTunings(Kc, tauI, tauD);
 
    setPoint_ = 0.0;
    processVariable_ = 0.0;
    prevProcessVariable_ = 0.0;
    controllerOutput_ = 0.0;
    prevControllerOutput_ = 0.0;
 
    
 
}
 
void PID::setInputLimits(float inMin, float inMax) {
 
    //Make sure we haven't been given impossible values.
    if (inMin >= inMax) {
        return;
    }
 
   
 
   
 
    inMin_ = inMin;
    inMax_ = inMax;
    inSpan_ = (inMax - inMin);
 
 
 
 
}
 
void PID::setOutputLimits(float outMin, float outMax) {
 
    //Make sure we haven't been given impossible values.
    if (outMin >= outMax) {
        return;
    }
 
    
 
   //ppp
 
    outMin_ = outMin;
    outMax_ = outMax;
    outMid_ = (outMin+outMax)/2;
    
    outSpan_ = (outMax - outMin);
   
    
    
    
    
 
}
 
 
 
//-------------------------------------------------- 
 
 
 
void PID::setTunings(float Kc, float tauI, float tauD) {
 
    //Verify that the tunings make sense.
    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
        return;
    }
 
    //Store raw values to hand back to user on request.
    pParam_ = Kc;
    iParam_ = tauI;
    dParam_ = tauD;
 
   
 
  
 
    Kc_ = Kc;
    tauI_=tauI;
    tauD_=tauD;
 
}


float PID::compute(float pv,  float sp) {
 
    
    processVariable_ = pv; //ce que l'on mesure
    setPoint_ = sp;  // ce que l'on veut atteindre
 
 
 
 
 
 
    //Pull in the input and setpoint, and scale them into percent span.
    float scaledPV = (processVariable_ - inMin_) / inSpan_;
 
    if (scaledPV > 1.0) {
        scaledPV = 1.0;
    }
    else if (scaledPV < 0.0) {
        scaledPV = 0.0;
    }
 
    float scaledSP = (setPoint_ - inMin_) / inSpan_;
    if (scaledSP > 1.0) {
        scaledSP = 1;
    }
    else if (scaledSP < 0.0) {
        scaledSP = 0;
    }
 
    float error = (scaledSP - scaledPV)*2;
// 100~ -100%
    



// if error add
 if ( (-1<error && error <1)             ) {
        accError_ = (accError_*0.66)+ error;
    }

     float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
  
     controllerOutput_= Kc_* error+(tauI_*accError_)+tauD_*dMeas+outMid_;
     de_ip=accError_;
     de_dp=dMeas;
     de_kp=error;
   
   
   
   
     
        
      if (controllerOutput_ < outMin_) {
        controllerOutput_ = outMin_;
    }
    else if (controllerOutput_ > outMax_) {
        controllerOutput_ = outMax_;
    }
   
   
   
   
   prevProcessVariable_ = scaledPV;
    
     
    return (controllerOutput_);
 
}
 
float PID::getInMin() {
 
    return inMin_;
 
}
 
float PID::getInMax() {
 
    return inMax_;
 
}
 
float PID::getOutMin() {
 
    return outMin_;
 
}
 
float PID::getOutMax() {
 
    return outMax_;
 
}
 
float PID::getInterval() {
 
    return tSample_;
 
}
 
float PID::getPParam() {
 
    return pParam_;
 
}
 
float PID::getIParam() {
 
    return iParam_;
 
}
 
float PID::getDParam() {
 
    return dParam_;
 
}