File content as of revision 3:e3b84ea63c0f:

/*
                        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_rl 1]), Ts, 'tustin')
      - Anti-Windup: Saturation of Ipart and u = P*e + IPart + DPart
*/

#include "PIDT2_Cntrl.h"

PIDT2_Cntrl::PIDT2_Cntrl(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(0.0f);
}

PIDT2_Cntrl::~PIDT2_Cntrl() {}

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

void PIDT2_Cntrl::setCoefficients(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(0.0f);
}

void PIDT2_Cntrl::setCoeff_P(float P) {
    this -> P = P;
}
void PIDT2_Cntrl::setCoeff_I(float I) {
    this -> I = I;
    this -> bi = I * Ts;
}
void PIDT2_Cntrl::setCoeff_D(float D) {
    this -> D = D;
    this -> bd = 2.0f * D / (Ts + 2.0f * tau_f);
}
void PIDT2_Cntrl::scale_PIDT2_param(float scale) {
    this -> P = P_init * scale;
    this -> I = I_init * scale;
    this -> D = D_init * scale;
    this -> bi = I_init * Ts * scale;
    this -> bd = 2.0f * D_init / (Ts + 2.0f * tau_f) * scale;
}

float PIDT2_Cntrl::update(float e) {
    IPart = saturate(IPart + bi * e, uMin, uMax);
    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 PIDT2_Cntrl::update(float e, float y) {

    IPart = saturate(IPart + bi * e, uMin, uMax);
    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;
}

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

float PIDT2_Cntrl::get_ulimit() {
    return this -> uMax;
}

float PIDT2_Cntrl::get_P_gain() {
    return this -> P;
}

float PIDT2_Cntrl::get_I() {
    return this -> I;
}

float PIDT2_Cntrl::get_D() {
    return this -> D;
}

float PIDT2_Cntrl::get_bd() {
    return this -> bd;
}

float PIDT2_Cntrl::get_ad() {
    return this -> ad;
}

float PIDT2_Cntrl::get_bi() {
    return this -> bi;
}

float PIDT2_Cntrl::get_bf() {
    return this -> bf;
}

float PIDT2_Cntrl::get_af() {
    return this -> bf;
}

void PIDT2_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;

    double P_d = static_cast < double > (P);
    double I_d = static_cast < double > (I);
    double D_d = static_cast < double > (D);

//    double tau_f_d = static_cast < double > (tau_f);
//    double tau_ro_d = static_cast < double > (tau_ro);

//    double Ts_d = static_cast < double > (Ts);

//    double bi_d = I_d * Ts_d;
//    this -> bi = static_cast < float > (bi_d);

//    double bd_d = 2.0 * D_d / (Ts_d + (2.0 * tau_f_d));
//    this -> bd = static_cast < float > (bd_d);

//    double ad_d = (Ts_d - 2.0 * tau_f_d) / (Ts_d + 2.0 * tau_f_d);
//    this -> ad = static_cast < float > (ad_d);

//    double bf_d = Ts_d / (Ts_d + 2.0 * tau_ro_d);
//    this -> bf = static_cast < float > (bf_d);

//    double af_d = (Ts_d - 2.0 * tau_ro_d) / (Ts_d + 2.0 * tau_ro_d);
//    this -> af = static_cast < float > (af_d);
	
	this -> bi = I * Ts;    
    this -> bd = 2.0f * D / (Ts + (2.0f * tau_f));
    this -> ad = (Ts - 2.0f * tau_f) / (Ts + 2.0f * tau_f);
    this -> bf = Ts / (Ts + 2.0f * tau_ro);
    this -> af = (Ts - 2.0f * tau_ro) / (Ts + 2.0f * tau_ro);
	

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

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