mecha10-behavior-runtime 0.9.0

//! Generic PID (proportional-integral-derivative) controller.
//!
//! Domain-agnostic: usable for any single-axis stabilization loop (altitude, attitude, velocity).

use serde::{Deserialize, Serialize};

/// Tunable gains for a [`PidController`]. Mirrors the field names/defaults of
/// `mecha10_controllers::motor::PidGains` for consistency, but is defined locally so this crate
/// doesn't take on a dependency on the driver-layer `controllers` crate just for three floats.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct PidGains {
    /// Proportional gain - reacts to the instantaneous error.
    pub kp: f64,
    /// Integral gain - reacts to the accumulated error over time (eliminates steady-state error).
    pub ki: f64,
    /// Derivative gain - reacts to the rate of change of the error (damps oscillation).
    pub kd: f64,
}

impl Default for PidGains {
    fn default() -> Self {
        Self {
            kp: 1.0,
            ki: 0.0,
            kd: 0.0,
        }
    }
}

/// A standard PID controller with output clamping and integral anti-windup.
///
/// Call [`PidController::update`] once per control tick with setpoint, measurement, and `dt`.
#[derive(Debug, Clone)]
pub struct PidController {
    gains: PidGains,
    /// Accumulated error, clamped to `integral_limits` each update (anti-windup).
    integral: f64,
    /// Error from the previous `update` call, used for the derivative term. `None` until the
    /// first call, so the derivative term is skipped (treated as 0) on that first call instead
    /// of producing a spurious spike from an assumed-zero previous error.
    previous_error: Option<f64>,
    /// Clamp applied to the final output, as `(min, max)`.
    output_limits: (f64, f64),
    /// Clamp applied to the accumulated integral term, as `(min, max)` - bounds how much
    /// "wind-up" can accumulate while the output is saturated at `output_limits`.
    integral_limits: (f64, f64),
}

impl PidController {
    /// Create a new controller with the given gains and output clamp. Integral wind-up is
    /// clamped to the same range as the output by default - see [`Self::with_integral_limits`]
    /// to widen/narrow that independently.
    pub fn new(gains: PidGains, output_limits: (f64, f64)) -> Self {
        Self {
            gains,
            integral: 0.0,
            previous_error: None,
            output_limits,
            integral_limits: output_limits,
        }
    }

    /// Override the integral anti-windup clamp independently of the output clamp.
    pub fn with_integral_limits(mut self, integral_limits: (f64, f64)) -> Self {
        self.integral_limits = integral_limits;
        self
    }

    /// Advance the controller by one tick and return the (clamped) control output.
    ///
    /// `dt` is the elapsed time in seconds since the previous call; a non-positive `dt` skips
    pub fn update(&mut self, setpoint: f64, measurement: f64, dt: f64) -> f64 {
        let error = setpoint - measurement;

        let derivative = if dt > 0.0 {
            self.integral = (self.integral + error * dt).clamp(self.integral_limits.0, self.integral_limits.1);
            let derivative = self
                .previous_error
                .map(|previous| (error - previous) / dt)
                .unwrap_or(0.0);
            self.previous_error = Some(error);
            derivative
        } else {
            0.0
        };

        let output = self.gains.kp * error + self.gains.ki * self.integral + self.gains.kd * derivative;
        output.clamp(self.output_limits.0, self.output_limits.1)
    }

    /// Clear accumulated integral/derivative state (e.g. when a behaviour restarts), so a
    /// stale integral from a previous run doesn't bias the next one.
    pub fn reset(&mut self) {
        self.integral = 0.0;
        self.previous_error = None;
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn proportional_only_tracks_error() {
        let gains = PidGains {
            kp: 2.0,
            ki: 0.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        let output = pid.update(10.0, 4.0, 0.1);
        assert_eq!(output, 12.0);
    }

    #[test]
    fn integral_accumulates_error_over_time() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        let first = pid.update(1.0, 0.0, 1.0);
        let second = pid.update(1.0, 0.0, 1.0);

        assert_eq!(first, 1.0);
        assert_eq!(second, 2.0);
    }

    #[test]
    fn derivative_reacts_to_rate_of_change_and_is_zero_on_first_call() {
        let gains = PidGains {
            kp: 0.0,
            ki: 0.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        let first = pid.update(10.0, 0.0, 1.0);
        assert_eq!(first, 0.0);

        let second = pid.update(10.0, 5.0, 1.0);
        assert_eq!(second, -5.0);
    }

    #[test]
    fn output_is_clamped_to_configured_limits() {
        let gains = PidGains {
            kp: 100.0,
            ki: 0.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-1.0, 1.0));

        assert_eq!(pid.update(10.0, 0.0, 1.0), 1.0);
        assert_eq!(pid.update(-10.0, 0.0, 1.0), -1.0);
    }

    #[test]
    fn integral_is_clamped_to_prevent_windup() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0)).with_integral_limits((-2.0, 2.0));

        for _ in 0..5 {
            pid.update(5.0, 0.0, 1.0);
        }

        assert_eq!(pid.update(5.0, 0.0, 1.0), 2.0);
    }

    #[test]
    fn non_positive_dt_skips_integral_and_derivative_terms() {
        let gains = PidGains {
            kp: 1.0,
            ki: 1.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        pid.update(10.0, 0.0, 1.0);
        let output = pid.update(10.0, 0.0, 0.0);

        assert_eq!(output, 20.0);
    }

    #[test]
    fn reset_clears_integral_and_derivative_state() {
        let gains = PidGains {
            kp: 0.0,
            ki: 1.0,
            kd: 1.0,
        };
        let mut pid = PidController::new(gains, (-100.0, 100.0));

        pid.update(5.0, 0.0, 1.0);
        pid.reset();

        let output = pid.update(5.0, 0.0, 1.0);
        assert_eq!(output, 5.0);
    }

    #[test]
    fn converges_to_setpoint_over_repeated_ticks() {
        let gains = PidGains {
            kp: 0.6,
            ki: 0.3,
            kd: 0.05,
        };
        let mut pid = PidController::new(gains, (-10.0, 10.0));

        let mut measurement = 0.0_f64;
        let setpoint = 5.0;
        let dt = 0.1;

        for _ in 0..200 {
            let output = pid.update(setpoint, measurement, dt);
            measurement += output * dt;
        }

        assert!(
            (measurement - setpoint).abs() < 0.1,
            "expected convergence near {setpoint}, got {measurement}"
        );
    }
}