File content as of revision 0:b5906c81772b:

#include "PIDController.hpp"

//PIDController::PIDController() {}

void PIDController::updateParams(ControllerParams &cp) {
    setKp(cp.kp);
    setTi(cp.ti);
    setTd(cp.td);
}

float PIDController::calculateCmd() const {
    float outputPWM;
    
    float Proportional = kp*dif(this->ref,this->out);
    float Integral;
    float Derivative = (kp*td/TE)*dif(this->ref,this->out);
    
    if(ti == 0) {
        outputPWM = Proportional + Derivative;
    } else {
        Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
        outputPWM = Proportional + Integral + Derivative;
    }
    
    if(outputPWM > 1) {
        outputPWM = outputPWM - Integral;
    } else if(outputPWM < 0) {
        outputPWM = 0;
    }
    
    return outputPWM;
}

void PIDController::setKp(float _kp) {
    kp = _kp;   
}

void PIDController::setTi(float _ti) {
    ti = _ti;
}

void PIDController::setTd(float _td) {
    td = _td;
}

float PIDController::getKp() const {
    return kp;
}

float PIDController::getTi() const {
    return ti;
}

float PIDController::getTd() const {
    return td;
}

PIDController::~PIDController() {}