File content as of revision 15:c70cad2f4e64:

/*
    DT1 Controller class
    everything is calculated in float

                     z - 1
      G(s) = Kd/tau ------- , p = 1 - Ts/tau   D corresponds Kd in Matlab-formlism pid(...)
                     z - p
*/

#include "DT1_Cntrl.h"
using namespace std;

DT1_Cntrl::DT1_Cntrl(float D, float tau_f, float Ts, float uMin, float uMax)
{
    setCoefficients(D, tau_f, Ts);
    set_limits(uMin, uMax);
    reset(0.0f);
}

DT1_Cntrl::~DT1_Cntrl() {}


void DT1_Cntrl::reset(float initValue)
{
    eold = initValue;
    yold = 0.0f;
}

float DT1_Cntrl::doStep(float e)
{
    float Dpart = D * ( e - eold ) + p * yold;
    if(Dpart > uMax) Dpart = uMax;
    else if(Dpart < uMin) Dpart = uMin;
    eold = e;
    yold = Dpart;
    return Dpart;
}

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

void DT1_Cntrl::setCoeff_D(float D)
{
    this->D = D/tau_f;
}

void DT1_Cntrl::setCoefficients(float D, float tau_f, float Ts)
{
    this->p = 1.0f - Ts/tau_f;
    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;
}