pid_controller.cpp

// pid_controller.cpp — PID with anti-windup and a filtered derivative.
//
// Python equivalent: simple-pid's PID(kp, ki, kd) with output_limits set.
#include "cppml/pid_controller.hpp"

#include "cppml/math_utils.hpp"

namespace cppml {

double PIDController::compute(double measurement) {
    return compute(measurement, cfg_.dt);
}

double PIDController::compute(double measurement, double dt) {
    const double error = setpoint_ - measurement;

    // --- Proportional ------------------------------------------------------
    const double p_term = cfg_.kp * error;

    // --- Integral with anti-windup ----------------------------------------
    // Accumulate, then clamp BEFORE it feeds the output. Without this clamp a
    // saturated actuator lets the integral grow unbounded ("windup"), causing
    // a large overshoot once the error finally reverses.
    integral_ += error * dt;
    integral_ = clamp(integral_, -cfg_.integral_limit, cfg_.integral_limit);
    const double i_term = cfg_.ki * integral_;

    // --- Derivative with low-pass filter ----------------------------------
    // The raw derivative amplifies measurement noise. A first-order filter
    // (time constant tau) smooths it; alpha = dt / (tau + dt) blends the new
    // raw derivative against the previous filtered value. tau = 0 disables it.
    double raw_derivative = 0.0;
    if (!first_step_ && dt > 0.0) raw_derivative = (error - prev_error_) / dt;
    if (cfg_.derivative_tau > 0.0) {
        const double alpha = dt / (cfg_.derivative_tau + dt);
        filtered_derivative_ += alpha * (raw_derivative - filtered_derivative_);
    } else {
        filtered_derivative_ = raw_derivative;
    }
    const double d_term = cfg_.kd * filtered_derivative_;

    prev_error_ = error;
    first_step_ = false;

    // --- Saturate the command ---------------------------------------------
    const double output = p_term + i_term + d_term;
    return clamp(output, -cfg_.max_output, cfg_.max_output);
}

void PIDController::reset() {
    integral_ = 0.0;
    prev_error_ = 0.0;
    filtered_derivative_ = 0.0;
    first_step_ = true;
}

}  // namespace cppml