File content as of revision 4:e3c9cb64be44:

#include "PID.h"

PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in){
    
    // Parameters
    Outputlimit_bool = true;
    Inputlimit_bool = false;
    AntiWindUp_bool = 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::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;
    }
    */
            
}
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
}