File content as of revision 12:81787539c2cb:

/*  
    PI Controller class with anti windup reset in biquad transposed direct form 2
    see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
    everything is calculated in float
    
                     Ts             z - 1             
      G(s) = P + I ------- + D/tau -------          D corresponds Kd in Matlab-formlism pid(...)
                    z - 1           z - p              
*/

#include "PID_Cntrl.h"
using namespace std;

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

PID_Cntrl::~PID_Cntrl() {}

void PID_Cntrl::reset(float initValue)
{
    Iold = initValue;
    eold = 0.0;yold = 0.0;Dpart_old = 0.0;
    del = 0.0;
}

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

}
void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
{
    this->p = 1.0f - Ts/tau_f;
    this->P = P;
    this->P_init = P;
    this->I = I;
    this->I_init = I;
    this->tau_f = tau_f;
    this->D_ = D/tau_f;     // modified D, now D is consistent with Matlab PID
    this->D__init = D/tau_f;     // modified D, now D is consistent with Matlab PID
    this->Ts = Ts;
    if(P!=0)
        this->Ka=1/P;
    else
        this->Ka=1.0f;
    
}
void PID_Cntrl::setCoeff_P(float P)
{
    this->P = P;
}
void PID_Cntrl::setCoeff_I(float I)
{
    this->I = I;
}
void PID_Cntrl::setCoeff_D(float D)
{
    this->D_ = D/this->tau_f;
}
void PID_Cntrl::scale_PID_param(float sc)
{
    this->P = this->P_init * sc;
    this->I = this->I_init * sc;
    this->D_ = this->D__init * sc;
}

float PID_Cntrl::doStep(float e)
{
    float Ipart = Iold+I*Ts*(e-del);
    float Dpart = D_*(e-eold)+p*Dpart_old;
    float u = P*e + Dpart  + Ipart;          // unconstrained output
    float uc = u;                // constrained output
    if(u > uMax) uc = uMax;
    else if(u < uMin) uc = uMin;
    del=(u-uc)*Ka;
    eold=e;
    Iold=Ipart;
    Dpart_old=Dpart;
    return uc;
}
float PID_Cntrl::doStep(float e,float y)
{
    float Ipart = Iold+I*Ts*(e-del);
    float Dpart = D_ * (y-yold) + p * Dpart_old;
    float u = P*e - Dpart  + Ipart;          // unconstrained output
    float uc = u;                // constrained output
    if(u > uMax) uc = uMax;
    else if(u < uMin) uc = uMin;
    del=(u-uc)*Ka;
    eold=e;
    Iold=Ipart;
    Dpart_old = Dpart;
    yold = y;
    return uc;
}

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

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

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