File content as of revision 1:bf62e74fbcf3:

/*  
    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 double
    
                     Ts         z - 1             
      G(s) = P + I ------- + D -------
                    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 uMax, float uMin)
{
    setCoefficients(P, I, D, tau_f, Ts);
    this->uMax = (double)uMax;
    this->uMin = (double)uMin;
    reset(0.0f);
}

PID_Cntrl::~PID_Cntrl() {}

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

void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
{
    this->p = 1.0 - (double)Ts/(double)tau_f;
    this->P = P;
    this->I = I;
    this->D = D;
    
}

float PID_Cntrl::doStep(double e)
{
    double u = P*e + D*(e-eold)+p*yold  +Iold+I*Ts*(e-del);          // unconstrained output
    double uc = u;                // constrained output
    if(u > uMax) uc = uMax;
    else if(u < uMin) uc = uMin;
    del=(u-uc)/P;
    return (float)uc;
}