Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

Lib_Cntrl/PID_Cntrl.cpp

Committer:
ernstpre
Date:
2021-08-24
Revision:
2:92c25cb669f4
Parent:
1:25a2b47ca291

File content as of revision 2:92c25cb669f4:

/*
*/

#include "PID_Cntrl.h"

PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
{
    this->P = P;
    this->I = I;
    this->D = D;
    this->tau_f = tau_f;
    this->Ts = Ts;
    this->uMin = uMin;
    this->uMax = uMax;
    reset(0);
    
    
}

PID_Cntrl::~PID_Cntrl() {}

void PID_Cntrl::reset(float initValue)
{
    Ipart = initValue;
    Dpart = 0.0f;
    e_old = 0.0f;
}


float PID_Cntrl::update(float e)
{
    // the main update 
    float Ppart = P * e;
    Ipart += I * Ts*e;
    Dpart =  Dpart*tau_f/(tau_f + Ts) + D/(tau_f + Ts)*(e - e_old);
    Ipart = saturate(Ipart);
    return saturate(Ppart + Ipart + Dpart);
}
float PID_Cntrl::saturate(float x)
{
    if(x > uMax)
        return x = uMax;
    else if (x < uMin)
        return x = uMin;
    return x;
}