File content as of revision 23:d6d4e8adf7fe:

#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.0005;
    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(int centerL, int centerR ,  int sp) {
    //turn right 122~64   122
    //turn left  64~6     8
       int  errorR = sp - centerR;
       int  errorL = sp - centerL;
       int error;
        if(centerL ==128 && centerR == 0){
            error = 0;
        }
        if (centerL == 128 && centerR != 0){
            error = -30;
        }
        else if (centerL != 128 && centerR == 0){
            error = 30;
        }
        else{
            if(errorR >= 0 && errorL >=0){
                if(errorR > errorL){
                    if(errorR > -5 && errorR < 5)
                        error = 0;
                    else 
                        error = errorR;
                }
                else{
                    if(errorL > -5 && errorL < 5)
                        error = 0;
                    else
                        error = errorL;
                }
                
            }
            else if(errorR < 0 && errorL >=0){
                if(-errorR > errorL){
                    if(errorR > -5 && errorR < 5)
                        error = 0;
                    else
                        error = errorR;
                }
                else{
                    if(errorL > -5 && errorL < 5)
                        error = 0;
                    else
                        error = errorL;
                }         
            }
             else if(errorR >= 0 && errorL <0){
                if(errorR > -errorL){
                    if(errorR > -5 && errorR < 5)
                        error = 0;
                    else
                        error = errorR;
                }
                else{ 
                    if(errorL > -5 && errorL < 5)
                        error = 0;
                    else 
                        error = errorL; 
                }         
            }
            else{
                if(errorR > errorL){
                    if(errorL > -5 && errorL < 5)
                        error = 0;
                    else
                        error = errorL;
                }
                else{
                    if(errorR > -5 && errorR < 5)
                        error = 0;
                    else 
                        error = errorR;
                }
            }
        }
        return 0.069 + Kp*error;
}

int PID::getCenter(int centerL, int centerR) {
    //turn right 122~64   122
    //turn left  64~6     8
    
    int result = 64;
    int errorR = 64 - centerR;
    int errorL = 64 - centerL;
    int negL = 1 , negR = 1;
    
    
        

        if(centerL == 128 && centerR == 0){// no black line

            result = 64;

        } else if(centerL == 128 && centerR != 0){// no left line

            result = centerR;


        } else if(centerL != 128 && centerR == 0){// no right line

            result = centerL;

        } else{// two black lines

            negL = (errorL >= 0) ? 1 : -1;
            negR = (errorR >= 0) ? 1 : -1;
            errorL *= negL;
            errorR *= negR;

            result = (errorL >= errorR) ? centerL : centerR;
        }
}
 
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_;
 
}