File content as of revision 2:1ded9d10f322:

/*
    PID-T1 Controller class

                      1           s
      G(s) = Kp + Ki --- + Kd ---------
                      s       T_f*s + p

    Eigther reseting the Nucleo via the black button or save a new software on 
    the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
    Therefor: NEVER !!! reset or save a new software while the VC is powered on
    (the green button on the VC is glowing green)                      

*/

#include "PID_Cntrl.h"
using namespace std;

PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
{
    // link member variables
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
    
    this->Tf = Tf;
    this->Ts = Ts;
    
    this->uMin = uMin;
    this->uMax = uMax;
    
    reset(0.0f);
}

PID_Cntrl::~PID_Cntrl() {}

void PID_Cntrl::reset(float initValue)
{

    // implement controller reset
    e_old = 0.0f;
    uI_old = initValue;
    uD_old = 0.0f;

}

float PID_Cntrl::update(double e)
{

    // controller update function 
    
    // calculate uI
    float uI = Ki * Ts * e + uI_old;
    
    // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
    if (uI > uMax)
        uI = uMax;
    else if (uI < uMin)
        uI = uMin;

    // calculate uD
    float uD = (Kd * (e - e_old) + Tf * uD_old) / (Tf + Ts);
    
    // calculate u
    float u = (Kp * e) + uI + uD;
    
    // saturate u, uMin <= u <= uMax
    if (u > uMax)
        u = uMax;
    else if (u < uMin)
        u = uMin;
    
    // update signal storage
    e_old = e;
    uI_old = uI;
    uD_old = uD;

    return u;
}