File content as of revision 10:eb29810d831b:

#include "PID_Cntrl.h"

/*
                    Ts
    C(z) = (P + I ---------- + D * tustin(s / (tau_f * s + 1))) * tustin(1 / (tau_ro * s + 1))
                 1 - z^-1
    - Corresponds to MATLAB command: C = pid(P, I, D, tau_f, Ts, 'IFormula', 'BackwardEuler', 'DFormula', 'Trapezoidal') * c2d(tf(1, [tau_ro 1]), Ts, 'tustin')
    - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart
*/

PID_Cntrl::PID_Cntrl(float I, float Ts, float uMin, float uMax)
{
    setup(I, Ts, uMin, uMax);
}

PID_Cntrl::PID_Cntrl(float P, float I, float Ts, float uMin, float uMax)
{
    setup(P, I, Ts, uMin, uMax);
}

PID_Cntrl::PID_Cntrl(float P, float I, float D, float Ts, float uMin, float uMax)
{
    setup(P, I, D, Ts, uMin, uMax);
}

PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
{
    setup(P, I, D, tau_f, Ts, uMin, uMax);
}

PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax)
{
    setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
}

PID_Cntrl::~PID_Cntrl() {}

void PID_Cntrl::reset()
{
    reset(0.0f);
}

void PID_Cntrl::reset(float initValue)
{
    IPart = initValue;
    Dpart = 0.0f;
    d_old = 0.0f;
    u_old = initValue;
    uf = initValue;
}

void PID_Cntrl::setup(float I, float Ts, float uMin, float uMax)
{
    P = 0.0f;
    D = 0.0f;
    double Ts_d = static_cast<double>( Ts );
    tau_f  = static_cast<float>( Ts_d/(0.95*pi) ); // unused
    tau_ro = 0.0f;
    setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
}

void PID_Cntrl::setup(float P, float I, float Ts, float uMin, float uMax)
{
    D = 0.0f;
    double Ts_d = static_cast<double>( Ts );
    tau_f  = static_cast<float>( Ts_d/(0.95*pi) ); // unused
    tau_ro = 0.0f;
    setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
}

void PID_Cntrl::setup(float P, float I, float D, float Ts, float uMin, float uMax)
{
    double Ts_d = static_cast<double>( Ts );
    tau_f  = static_cast<float>( Ts_d/(0.95*pi) ); // 1.0/(0.95*2.0*pi*fnyq)
    tau_ro = 0.0f;
    setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
}

void PID_Cntrl::setup(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
{
    tau_ro = 0.0f;
    setup(P, I, D, tau_f, tau_ro, Ts, uMin, uMax);
}

void PID_Cntrl::setup(float P, float I, float D, float tau_f, float tau_ro, float Ts, float uMin, float uMax)
{
    setCoefficients(P, I, D, tau_f, tau_ro, Ts);
    this->uMin = uMin;
    this->uMax = uMax;
    reset();
}

void PID_Cntrl::setCoeff_P(float P)
{
    this->P = P;
}

void PID_Cntrl::setCoeff_I(float I)
{
    this->I = I;
    updateCoeff_I(I, Ts);
}

void PID_Cntrl::setCoeff_D(float D)
{
    this->D = D;
    updateCoeff_D(D, Ts, tau_f);
}

void PID_Cntrl::scale_PIDT2_param(float scale)
{
    P = P_init * scale;
    I = I_init * scale;
    D = D_init * scale;
    updateCoeff_I(I, Ts);
    updateCoeff_D(D, Ts, tau_f);
}

float PID_Cntrl::update(float e)
{
    if(bi !=0) {
        IPart = saturate(IPart + bi*e, uMin, uMax);
    } else {
        IPart = 0.0f;
    }
    Dpart = bd*(e - d_old) - ad*Dpart;
    d_old = e;
    float u = P*e + IPart + Dpart;
    uf = saturate(bf*(u + u_old) - af*uf, uMin, uMax);
    u_old = u;
    return uf;
}

float PID_Cntrl::update(float e, float y)
{
    if(bi !=0) {
        IPart = saturate(IPart + bi*e, uMin, uMax);
    } else {
        IPart = 0.0f;
    }
    Dpart = bd*(y - d_old) - ad*Dpart;
    d_old = y;
    float u = P*e + IPart - Dpart;
    uf = saturate(bf*(u + u_old) - af*uf, uMin, uMax);
    u_old = u;
    return uf;
}

bool PID_Cntrl::update_param(int cntrlr_param, float value)
{
    switch (cntrlr_param) {
        case 0:
            setCoeff_P(value);
            return true;
            break;
        case 1:
            setCoeff_I(value);
            return true;
            break;
        case 2:
            setCoeff_D(value);
            return true;
            break;
        case 3:
            this->tau_f = value;
            break;
        case 4:
            this->tau_ro = value;
            break;
        case 5:
            return false;
            break;
        case 6:
            this->uMin = value;
            return true;
            break;
        case 7:
            this->uMax = value;
            return true;
            break;
        default:
            return false;
            break;
    }
    updateCoeff_I(I, Ts);
    updateCoeff_D(D, Ts, tau_f);
    updateCoeff_RO(Ts, tau_ro);
    return true;
}

void PID_Cntrl::set_limits(float uMin, float uMax)
{
    this->uMin = uMin;
    this->uMax = uMax;
}

float PID_Cntrl::prewarp(float T, float Ts)
{
    double T_d  = static_cast<double>( T );
    double Ts_d = static_cast<double>( Ts );
    return static_cast<float>( Ts_d/(2.0*tan(Ts_d/(2.0*T_d))) );
}

float PID_Cntrl::get_ulimit()
{
    return uMax;
}

float PID_Cntrl::get_P_gain()
{
    return P;
}

float PID_Cntrl::get_bd()
{
    return bd;
}

float PID_Cntrl::get_ad()
{
    return ad;
}

void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float tau_ro, float Ts)
{
    /* store parameters */
    this->P = P;
    this->I = I;
    this->D = D;
    this->tau_f = tau_f;
    this->tau_ro = tau_ro;
    this->Ts = Ts;
    updateCoeff_I(I, Ts);
    updateCoeff_D(D, Ts, tau_f);
    updateCoeff_RO(Ts, tau_ro);

    /* store initial parameters */
    this->P_init = P;
    this->I_init = I;
    this->D_init = D;
}

float PID_Cntrl::saturate(float u, float uMin, float uMax)
{
    if(u > uMax) {
        u = uMax;
    } else if(u < uMin) {
        u = uMin;
    }
    return u;
}

void PID_Cntrl::updateCoeff_I(float I, float Ts)
{
    double I_d  = static_cast<double>( I  );
    double Ts_d = static_cast<double>( Ts );
    bi = static_cast<float>( I_d*Ts_d );
}

void PID_Cntrl::updateCoeff_D(float D, float Ts, float tau_f)
{
    double D_d     = static_cast<double>( D  );
    double Ts_d    = static_cast<double>( Ts );
    double tau_f_d = static_cast<double>( tau_f );
    bd = static_cast<float>( 2.0*D_d/(Ts_d + 2.0*tau_f_d) );
    ad = static_cast<float>( (Ts_d - 2.0*tau_f_d)/(Ts_d + 2.0*tau_f_d) );
}

void PID_Cntrl::updateCoeff_RO(float Ts, float tau_ro)
{
    double Ts_d     = static_cast<double>( Ts );
    double tau_ro_d = static_cast<double>( tau_ro );
    bf = static_cast<float>( Ts_d/(Ts_d + 2.0*tau_ro_d) );
    af = static_cast<float>( (Ts_d - 2.0*tau_ro_d)/(Ts_d + 2.0*tau_ro_d) );
}