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
    
                  Tn*s + 1                      
      G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
                    Ts*s                     
*/

#include "PI_Cntrl.h"

using namespace std;

PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
{
    setCoefficients(Kp, Tn, Ts);
    uMax = 10000000000.0;
    uMin = -uMax;
    reset(0.0f);
}

PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
{
    setCoefficients(Kp, Tn, Ts);
    this->uMax = (double)uMax;
    uMin = -(double)uMax;
    reset(0.0f);
}

PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
{
    setCoefficients(Kp, Tn, Ts);
    this->uMax = (double)uMax;
    this->uMin = (double)uMin;
    reset(0.0f);
}

PI_Cntrl::~PI_Cntrl() {}

void PI_Cntrl::reset(float initValue)
{
    s = (double)initValue;
}

void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
{
    b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
    b1 = -(double)Kp;
    b2 = (double)Ts/(double)Tn;    
}

float PI_Cntrl::doStep(double e)
{
    double u = b0*e + s;          // unconstrained output
    double uc = u;                // constrained output
    if(u > uMax) uc = uMax;
    else if(u < uMin) uc = uMin;
    s = b1*e + u - b2*(u - uc);
    return (float)uc;
}