thk_pid_controller.cpp

#include <thk_pid_controller.h>

thk_PIDController::thk_PIDController(double Kp, double Ki, double Kd, double setpoint) : Kp(Kp), Ki(Ki), Kd(Kd), setpoint(setpoint) {}

void thk_PIDController::set_setpoint(double setpoint)
{
    this->setpoint = setpoint;
}

double thk_PIDController::compute(double input_value)
{
    double error_value = setpoint - input_value;

    out_p = Kp * error_value;  // P-Anteil
    out_i += Ki * error_value; // I-Anteil
    out_d -= Kd * error_value; // D-Anteil

    return out_p + out_i + out_d;
}

void thk_PIDController::reset()
{
    out_i = 0;
    out_d = 0;
}