File content as of revision 7:52f66036f628:

#include "PID.h"

PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in){

    // Parameters
    Outputlimit_bool = false; // true
    Inputlimit_bool = false;
    AntiWindUp_bool = false; // true
    outputLimits_H = 7.4;
    outputLimits_L = -7.4;
    inputLimits_H = 0.0;
    inputLimits_L = 0.0;
    feedbackvalue = 0.0;

    // Feedback gain
    Kp = Kp_in;
    Ki = Ki_in;
    Kd = Kd_in;
    // Ka = 0.1; // Volt.-sec./deg.
    Ka = 0.0017;// Volt.-sec./rad

    // Sampling time
    Ts = Sampletime_in;

    // Variables
    error[0] = 0.0;
    error[1] = 0.0;
    error_I = 0.0;
    //
    reference = 0.0;
    output = 0.0;



}

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_in)
{
    AntiWindUp_bool = true;
    Ka = Ka_in;
}
void PID::Reset(void){
    error[0] = 0.0;
    error[1] = 0.0;
    error_I = 0.0;
    output = 0.0;
}
void PID::Compute(float reference_in, float feedbackvalue_in){

    if(Inputlimit_bool == true){
        if( reference_in > inputLimits_H){
            reference = inputLimits_H;
        }else if( reference_in < inputLimits_L){
            reference = inputLimits_L;
        }else{
            reference = reference_in;
        }

    }else{
        reference = reference_in;
    }

    // bypass
    feedbackvalue = feedbackvalue_in;

    error[0] = reference - feedbackvalue;
    // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
    output = Kp*error[0] + Ki*error_I;

    // Delay 1
    error[1] = error[0];

    // Integration
    error_I += Ts*error[0];

    // Output satuation
    if(Outputlimit_bool && AntiWindUp_bool){
        if( output >= outputLimits_H){
            // output = output - (output - outputLimits_H)*Ka;
            error_I -= Ka*(output - outputLimits_H); // Anti-windup
            output = outputLimits_H;
        }else if( output <= outputLimits_L){
            // output = output - (output - outputLimits_L)*Ka;
            error_I -= Ka*(output - outputLimits_L); // Anti-windup
            output = outputLimits_L;
        }
    }else{
        // output = output;
    }

}
void PID::Compute_noWindUP(float reference_in, float feedbackvalue_in){

    if(Inputlimit_bool == true){
        if( reference_in > inputLimits_H){
            reference = inputLimits_H;
        }else if( reference_in < inputLimits_L){
            reference = inputLimits_L;
        }else{
            reference = reference_in;
        }

    }else{
        reference = reference_in;
    }

    // bypass
    feedbackvalue = feedbackvalue_in;

    error[0] = reference - feedbackvalue;
    // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
    output = Kp*error[0] + Ki*error_I;

    // Delay 1
    error[1] = error[0];

    // Integration
    error_I += Ts*error[0];

    // Output satuation
    /*
    if(Outputlimit_bool && AntiWindUp_bool){
        if( output >= outputLimits_H){
            // output = output - (output - outputLimits_H)*Ka;
            error_I -= Ka*(output - outputLimits_H); // Anti-windup
            output = outputLimits_H;
        }else if( output <= outputLimits_L){
            // output = output - (output - outputLimits_L)*Ka;
            error_I -= Ka*(output - outputLimits_L); // Anti-windup
            output = outputLimits_L;
        }
    }else{
        // output = output;
    }
    */

}

// Use separately
// Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- Anti_windup(delta) -- output
//////////////////////////////////////////////////
void PID::Saturation_output(){
        if( output >= outputLimits_H){
            delta_output = outputLimits_H - output;
            output = outputLimits_H;
        }else if( output <= outputLimits_L){
            delta_output = outputLimits_L - output;
            output = outputLimits_L;
        }else{
            delta_output = 0.0;
        }
}
void PID::Anti_windup(float delta){ // delta_V = Vs - V
    // Anti-windup compensation
    error_I += Ka*delta; // Anti-windup
}
////////////////////////////////////////////////// end Use separately

// Use alone
// Compute_noWindUP() -- Anti_windup() -- output
//////////////////////////////////////////////////
void PID::Anti_windup(){ // delta_V = Vs - V

    PID::Saturation_output();
    // Anti-windup compensation
    error_I += Ka*PID::delta_output; // Anti-windup
}
////////////////////////////////////////////////// end Use alone