File content as of revision 3:dccd51230bf5:

#include "PID.h"

PID::PID(float setKp, float setKi, float setKd,  float setSampletime){
    
    Kp = 0.0;
    Ki = 0.0;
    Kd = 0.0;
    error[0] = 0.0;
    error[1] = 0.0;
    error[2] = 0.0;
    output = 0.0;
//    pidOutput = 0.0;
    reference = 0.0;
    sampletime = 0.0;
    Outputlimit_bool = false;
    Inputlimit_bool = false;
    AntiWindUp_bool = false;
    outputLimits_H = 0.0;
    outputLimits_L = 0.0;
    inputLimits_H = 0.0;
    inputLimits_L = 0.0; 
    feedbackvalue = 0.0;   
    
    Kp = setKp;
    Ki = setKi*setSampletime;
    Kd = setKd/setSampletime;
    sampletime = setSampletime;   
}

void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
    Outputlimit_bool = true;
    outputLimits_H = setoutputLimits_H;
    outputLimits_L = setoutputLimits_L;    
}

void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
    Inputlimit_bool = true;
    inputLimits_H = setinputLimits_H;
    inputLimits_L = setinputLimits_L;     
}

void PID::EnableAntiWindUp(float Ka_)
{
    AntiWindUp_bool = true;
    Ka = Ka_;
}

void PID::Compute(float setreference, float setfeedbackvalue){

    if(Inputlimit_bool == true){
        if( setreference >= inputLimits_H){
            reference = inputLimits_H;
        }else if( setreference <= inputLimits_L){
            reference = inputLimits_L;
        }
    }else{
        reference = setreference;
    }
    
    feedbackvalue = setfeedbackvalue;
    
    error[0] = reference - feedbackvalue;
//    pidOutput = ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2];
    output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0f*Kd )*error[1] + Kd*error[2];
    error[2] = error[1];
    error[1] = error[0];    
    
    if(Outputlimit_bool == true && AntiWindUp_bool == true){
        if( output >= outputLimits_H){
            output = output - (output - outputLimits_H)*Ka;
            //output = outputLimits_H;
        }
        else if( output <= outputLimits_L){
            output =output - (output - outputLimits_L)*Ka;
            //output = outputLimits_L;  
        }
    }else{
        output = output;
    }
            
}