File content as of revision 7:6f0e5de35b48:

#include "PID.h"

PID::PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in):
        derivative_error(Sampletime_in),
        SAT_command(1,-1),
        SAT_output(1,-1)
{
    // To enble, run this->start() function
    enable = false;

    // Sampling time
    Ts = Sampletime_in;


    // Parameters
    is_limiting_command = false;
    is_limiting_output = false; // true
    //
    is_using_integral = false; // Determine if the integral control is going to be used.
    is_using_derivative = false; // Determine if the derivative control is going to be used.
    //
    is_using_outSource_d_error = false; // Determine whether using the signal for d_error or using numerical derivative to derive d_error from error.
    //
    is_antiWindUp = false; // true

    // Parameters
    // Feedback gain
    Kp = Kp_in;
    Ki = Ki_in;
    Kd = Kd_in;
    //
    if (Ki_in == 0.0){
        is_using_integral = false;
    }else{
        is_using_integral = true;
    }
    //
    if (Kd_in == 0.0){
        is_using_derivative = false;
    }else{
        is_using_derivative = true;
    }
    //
    // Ka = 0.0017;// Volt.-sec./rad
    //
    // Small over-bound value for numerical stability
    overBound_value = 0.01; // Small positive value, adjustable


    // States
    error = 0.0;
    d_error = 0.0;
    error_int = 0.0;
    //
    error_int_increment = 0.0;

    // Input signal
    command = 0.0;
    feedbackValue = 0.0;
    // Output signal
    output = 0.0;
    // Error by saturation
    delta_output = 0.0; // Error by saturation

}
// Process controller
void PID::start(){ // Run
    if (enable){
        return;
    }
    //
    enable = true;
}
void PID::pause(){ // Stop updating but no reset
    //
    enable = false;
}
void PID::stop(){ // Stop and reset
    if (!enable){
        return;
    }
    //
    enable = false;
    reset();
}
void PID::reset(void){
    // States
    error = 0.0;
    d_error = 0.0;
    error_int = 0.0;
    //
    error_int_increment = 0.0;

    // Input signal
    command = 0.0;
    feedbackValue = 0.0;
    // Output signal
    output = 0.0;
    // Error by saturation
    delta_output = 0.0; // Error by saturation

    // Reset the derivative
    derivative_error.reset(0.0);
}
//
void PID::EnableAntiWindUp(float Ka_in)
{
    is_antiWindUp = true;
}
//
void PID::set_PID_gains(float Kp_in, float Ki_in, float Kd_in){ // Setting Kp, Ki, and Kd
    Kp = Kp_in;
    Ki = Ki_in;
    Kd = Kd_in;
    //
    if (Ki_in == 0.0){
        is_using_integral = false;
    }else{
        is_using_integral = true;
    }
    //
    if (Kd_in == 0.0){
        is_using_derivative = false;
    }else{
        is_using_derivative = true;
    }
}
void PID::SetInputLimits(float inputLimits_H_in, float inputLimits_L_in){
    is_limiting_command = true;
    //
    SAT_command.set_bound(inputLimits_H_in, inputLimits_L_in);
}
void PID::SetOutputLimits(float outputLimits_H_in, float outputLimits_L_in){
    is_limiting_output = true;
    // is_antiWindUp = true;
    //
    SAT_output.set_bound(outputLimits_H_in, outputLimits_L_in);

    //
    // Set the over-bound value to be 1% of peak-peak range
    overBound_value = 0.001*(outputLimits_H_in - outputLimits_L_in);
}

// Main function for computing the PID
//--------------------------------------------------//
void PID::set_d_error(float d_error_in){ // Insert d_error before iteration.
    d_error = d_error_in;
    is_using_outSource_d_error = true;
}
//
void PID::iterateOnce(float command_in, float feedbackValue_in){
    // Main process
    iterateOnce_noAntiWindUP(command_in, feedbackValue_in);

    // Output satuation
    if(is_limiting_output){
        if (is_antiWindUp){
            // Output saturation + anti-windup
            this->Saturation_AntiWindUp();
        }else{
            // Output saturation only
            this->Saturation_output();
        }
        //
    }else{
        // output = output;
    }

}
//
void PID::iterateOnce_noAntiWindUP(float command_in, float feedbackValue_in){

    //
    // -- Important! --
    // Post-integral action:
    // This integral action generates the error_int of time (k-1) (previous time), which means the back-step integral.
    // The actual integral action move to here is for implementing the (digital-version) anti-windup.
    if (is_using_integral){
        error_int += error_int_increment;
    }else{
        error_int = 0.0;
    }
    //

    // Processing input signals
    //----------------------------------------//
    // Comand saturation
    if(is_limiting_command){
        // Saturation
        command = SAT_command.filter(command_in);
        //
    }else{
        command = command_in;
    }

    // bypass the feedback value
    feedbackValue = feedbackValue_in;
    //----------------------------------------//

    // PID control
    //----------------------------------------//
    // Calculating the error
    error = command - feedbackValue;
    //
    if (is_using_derivative){
        if (is_using_outSource_d_error){
            // Please use the insert function for d_error.
        }else{
            // Calculating the derivative of error
            d_error = derivative_error.filter(error);
        }
        // Control output
        output = Kp*error + Ki*error_int + Kd*d_error;
    }else{
        // Control output
        output = Kp*error + Ki*error_int;
    }
    //----------------------------------------//

    // Pre-integral action
    error_int_increment = Ts*error;

}
//--------------------------------------------------//


// Used separately
// Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- AntiWindUp(delta) -- output
//////////////////////////////////////////////////
void PID::Saturation_output(){
    //
    output = SAT_output.filter(output);
    delta_output = SAT_output.delta_out; // (original_out - limited_out)
}
void PID::AntiWindUp(float delta){ // delta_V = V - V_sat

    /*
    // (Analog) Anti-windup compensation
    // error_int -= Ka*delta; // Anti-windup
    error_int_increment -= Ka*delta; // Anti-windup
    */

    // (Digital) Anti-windup
    // If the output is going to be over bound, stop the integral action in that direction
    if (Ki > 0.0){
        //
        if (delta > overBound_value && error_int_increment > 0.0){ // Positive saturation
            error_int_increment = 0.0;
        }else if (delta < -overBound_value && error_int_increment < 0.0){ // Negative saturation
            error_int_increment = 0.0;
        }
    }else if (Ki < 0.0){
        //
        if (delta > overBound_value && error_int_increment < 0.0){ // Positive saturation
            error_int_increment = 0.0;
        }else if (delta < -overBound_value && error_int_increment > 0.0){ // Negative saturation
            error_int_increment = 0.0;
        }
    }

}
////////////////////////////////////////////////// end Use separately

// Used alone
// Compute_noWindUP() -- Saturation_AntiWindUp() -- output
//////////////////////////////////////////////////
void PID::Saturation_AntiWindUp(){ // delta_V = V - V_sat
    //
    // Output saturation
    this->Saturation_output();
    // Anti-windup compensation
    this->AntiWindUp(delta_output);
}
////////////////////////////////////////////////// end Use alone