File content as of revision 16:7ada30380595:

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


PID::PID(float in_min,float in_max,float out_min,float out_max,float Kp, float Ki, float Kd) {
//in_min in_out  camera array
//out_min out_max servo range
//
    usingFeedForward = false;
    //inAuto = false;
 
    //Default the limits to the full range of I/O.
    //Make sure to set these to more appropriate limits for your application.
 
 //BX tune
    /*setInputLimits(in_min,in_max);
    setOutputLimits(out_min,out_max);
 
    tSample_ = interval;// init is 10
 
    setTunings(Kp, Ki, Kd);
 
    setPoint_ = 0.0;
    processVariable_ = 0.0;
    prevProcessVariable_ = 0.0;
    controllerOutput_ = 0.0;
    prevControllerOutput_ = 0.0;
 
    accError_ = 0.0;
    bias_ = 0.0;
 
    realOutput_ = 0.0;*/
 
}
 
void PID::setInputLimits(float inMin, float inMax) {
 
    //Make sure we haven't been given impossible values.
    if (inMin >= inMax) {
        return;
    }
 
    //Rescale the working variables to reflect the changes.
    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
    accError_ *= (inMax - inMin) / inSpan_;
 
    //Make sure the working variables are within the new limits.
    if (prevProcessVariable_ > 1) {
        prevProcessVariable_ = 1;
    }
    else if (prevProcessVariable_ < 0) {
        prevProcessVariable_ = 0;
    }
 
    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;
    }
 
    //Rescale the working variables to reflect the changes.
    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
 
    //Make sure the working variables are within the new limits.
    if (prevControllerOutput_ > 1) {
        prevControllerOutput_ = 1;
    }
    else if (prevControllerOutput_ < 0) {
        prevControllerOutput_ = 0;
    }
 
    outMin_ = outMin;
    outMax_ = outMax;
    outSpan_ = outMax - outMin;
 
}
 
 
 
//-------------------------------------------------- 
 
 
 
void PID::setTunings(float Kp, float Ki, float Kd) {
 
    //Verify that the tunings make sense.
    if (Kp == 0.0 || Ki < 0.0 || Kd < 0.0) {
        return;
    }
 
    //Store raw values to hand back to user on request.
    pParam_ = Kp;
    iParam_ = Ki;
    dParam_ = Kd;
 
    float tempTauR;
 
    if (Ki == 0.0) {
        tempTauR = 0.0;
    }
    else {
        tempTauR = (1.0 / Ki) * tSample_;
    }
 
    //For "bumpless transfer" we need to rescale the accumulated error.
    //if (inAuto) {
        //if (tempTauR == 0.0) {
            //accError_ = 0.0;
        //}
        //else {
            accError_ *= (Kp_ * tauR_) / (Kp * tempTauR);
        //}
    //}
 
    Kp_ = Kp;
    tauR_ = tempTauR;
    Kd_ = Kd / tSample_;
 
}
 
void PID::reset(void) {
 
    float scaledBias = 0.0;
 
    if (usingFeedForward) {
        scaledBias = (bias_ - outMin_) / outSpan_;
    }
    else {
        scaledBias = (realOutput_ - outMin_) / outSpan_;
    }
 
    prevControllerOutput_ = scaledBias;
    prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
 
    //Clear any error in the integral.
    accError_ = 0;
 
}
/*
void PID::setMode(int mode) {
 
    //We were in manual, and we just got set to auto.
    //Reset the controller internals.
    if (mode != 0 && !inAuto) {
        reset();
    }
 
    inAuto = (mode != 0);
 
}*/
 
void PID::setInterval(float interval) {
 
    if (interval > 0) {
        //Convert the time-based tunings to reflect this change.
        tauR_ *= (interval / tSample_);
        accError_ *= (tSample_ / interval);
        Kd_ *= (interval / tSample_);
        tSample_ = interval;
    }
 
}
/*
void PID::setSetPoint(float sp) {
 
    setPoint_ = sp;
 
}
 
void PID::setProcessValue(float pv) {
 
    processVariable_ = pv;
 
}
*/
void PID::setBias(float bias){
 
    bias_ = bias;
    usingFeedForward = 1;
 
}
 
float PID::compute(float R ,float L,  float sp) {
    //pv centerR centerL
    //enregistrer variables dans var interne
    float centerR = R;  //(right + left) / 2
    float centerL = L; 
    float centerB = sp;  // center of black
    float error = centerB - centerR;
    if(error > -8 && error <8){
        return Kp;
    }
    else if((error < -8 && error > -20)||(error > 8 && error < 40)){
        //return 0.085 + Kp*error;
        return Kp;
    }
    else return Kp;
    
    
   
 
}
 
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_;
 
}