File content as of revision 7:f04bde0ca846:

#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
    Kp = 0.0004;
    Ki = 0.0;
    Kd = 0.0;
    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 center ,  float sp) {
    //turn right 122~64   122
    //turn left  64~6     8
    float C = center;
    float goal = sp;  // center of black
    float error = goal - C;//
    return 0.073+Kp*error;
 
}
 
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_;
 
}