File content as of revision 0:937360eb9f8c:

#include "PT1_Controller.h"

PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf)
{
    copyControllerPara(Kp, Ts, Tf);
    calcFilterCoeff();
    u_max = 1000000.0; // big number
    u_min = -u_max;
}

PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max)
{
    copyControllerPara(Kp, Ts, Tf);
    calcFilterCoeff();
    this->u_max = u_max;
    u_min = -u_max;
}

PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max, double u_min)
{
    copyControllerPara(Kp, Ts, Tf);
    calcFilterCoeff();
    this->u_max = u_max;
    this->u_min = u_min;
}

PT1_Controller::~PT1_Controller() {}

void PT1_Controller::reset()
{
    u_kmin1 = 0.0;
    uf_k = 0.0;
}

void PT1_Controller::reset(double setVal)
{
    u_kmin1 = setVal;
    uf_k = setVal;
}

double PT1_Controller::update(double e)
{
    // proportional controller
    double u = Kp * e;

    // constrained low-pass filter
    uf_k = u*bf + u_kmin1*bf - af*uf_k;
    u_kmin1 = u;
    return saturate(uf_k);
}

void PT1_Controller::copyControllerPara(double Kp, double Ts, double Tf)
{
    this->Kp = Kp;
    this->Ts = Ts;
    this->Tf = Tf;
}

void PT1_Controller::calcFilterCoeff()
{
    // tustin transformation
    bf = Ts / (2.0*Tf + Ts);
    af = (Ts - 2.0*Tf) / (2.0*Tf + Ts);
}

double PT1_Controller::saturate(double u)
{
    if(u > u_max) {
        u = u_max;
    } else if(u < u_min) {
        u = u_min;
    }
    return u;
}