autocore-std 3.3.43

//! Closed-loop pressure / force controller for Profile Position axes.
//!
//! Drives a single axis toward a target load (e.g. from a load cell) and
//! holds it there by issuing a small incremental `move_absolute` every
//! tick. Uses an exponential moving-average filter on the load input, a
//! PID with anti-windup, a per-tick step clamp, and optional position
//! limits to keep the axis inside a safety envelope.
//!
//! # Lifecycle
//!
//! Same shape as every other domain FB in autocore-std:
//!
//! ```text
//!   [Idle]  --start(target, max_load)-->  [Controlling]
//!   [Controlling]  --stop()----------->   [Halted] -- axis idle --> [Idle]
//!   [Controlling]  --fault or limit -->   [Error]
//! ```
//!
//! `is_busy()` is `true` while the loop is running or stopping. `is_error()`
//! goes true on axis fault or `max_load` exceedance; call `error_message()`
//! for a short description, or inspect the underlying state machine.
//!
//! # Engage only when the axis has settled
//!
//! `start()` lazily seeds `commanded_position = axis.position()` on the
//! first `tick()` after engagement. If you engage while the axis is still
//! decelerating from a prior move, the seed lands in the middle of that
//! deceleration profile and the loop's initial output will fight the
//! drive's trajectory. The typical pattern is: run `MoveToLoad` to target,
//! wait for it to go idle, THEN call `start()`.

use crate::fb::StateMachine;
use super::axis_view::AxisHandle;

/// Configuration for the Pressure Control function block.
#[derive(Debug, Clone)]
pub struct PressureControlConfig {
    /// Proportional gain (Kp).
    pub kp: f64,
    /// Integral gain (Ki).
    pub ki: f64,
    /// Derivative gain (Kd).
    pub kd: f64,
    /// Feed forward value added directly to the output.
    pub feed_forward: f64,
    /// Maximum allowed position delta (in user units) per tick.
    /// Critical for safety to prevent crushing the load cell.
    /// E.g., 0.001 inches per ms tick.
    pub max_step: f64,
    /// Sub-threshold deadband. When the computed step is smaller than
    /// `min_step` we skip the `move_absolute` call entirely. Keeps the
    /// EtherCAT bus quiet near steady state. Set to 0 to issue a move
    /// every tick regardless of step size.
    pub min_step: f64,
    /// Maximum accumulated integral windup.
    pub max_integral: f64,
    /// Exponential Moving Average filter coefficient for the load cell (0.0 to 1.0).
    /// 1.0 = No filtering (raw data).
    /// 0.1 = Heavy filtering.
    pub filter_alpha: f64,
    /// If true, a positive error (need more pressure) results in a *negative* move.
    /// Set to true if moving the axis down (negative) increases compression.
    pub invert_direction: bool,
    /// The acceptable load error window to be considered "in tolerance" (e.g., +/- 2.0 lbs).
    pub tolerance: f64,
    /// How long the load must remain within `tolerance` before reporting `in_tolerance = true`.
    pub settling_time: f64,
    /// Optional hard ceiling on `commanded_position` in user units. If the
    /// loop's integrator drives the commanded position past this, the
    /// commanded position is clamped and the integral is frozen that tick
    /// (extra anti-windup). `None` disables the clamp.
    pub position_limit_pos: Option<f64>,
    /// Optional hard floor on `commanded_position`. Mirror of
    /// `position_limit_pos`.
    pub position_limit_neg: Option<f64>,
}

impl Default for PressureControlConfig {
    fn default() -> Self {
        Self {
            kp: 0.0,
            ki: 0.0,
            kd: 0.0,
            feed_forward: 0.0,
            max_step: 0.005,      // Conservative 5 thousandths max step
            min_step: 0.0,        // Opt-in deadband; default fires every tick
            max_integral: 100.0,
            filter_alpha: 0.5,
            invert_direction: false,
            tolerance: 1.0,
            settling_time: 0.1,
            position_limit_pos: None,
            position_limit_neg: None,
        }
    }
}

#[repr(i32)]
#[derive(Copy, Clone, PartialEq, Debug)]
enum PcState {
    Idle        = 0,
    Controlling = 10,
    /// `stop()` issued; axis has been halted, waiting on the drive to
    /// flip its busy bit back to false before returning to Idle.
    Halted      = 20,
    Error       = 30,
}

impl PcState {
    fn from_index(idx: i32) -> Option<Self> {
        match idx {
            x if x == Self::Idle as i32        => Some(Self::Idle),
            x if x == Self::Controlling as i32 => Some(Self::Controlling),
            x if x == Self::Halted as i32      => Some(Self::Halted),
            x if x == Self::Error as i32       => Some(Self::Error),
            _ => None,
        }
    }
}

/// A closed-loop PID pressure/force controller for Profile Position (PP) axes.
#[derive(Debug, Clone)]
pub struct PressureControl {
    /// Internal state machine.
    state: StateMachine,

    /// True when the current load has been within `config.tolerance` of the
    /// target for at least `config.settling_time` seconds. Cleared by
    /// `start()`, `stop()`, and any error transition.
    in_tolerance: bool,

    // Latched on start().
    target_load: f64,
    max_load: f64,

    // Internal PID state.
    integral: f64,
    prev_error: f64,
    filtered_load: f64,

    // Internal setpoint state. Accumulated to avoid the positive-feedback
    // lag of re-reading axis.position() each tick. Reseeded from the axis
    // on the first tick after start().
    commanded_position: f64,

    /// PID error from the most recent tick, exposed via `pid_error()`.
    last_pid_error: f64,

    // Tolerance timer.
    settling_timer: f64,

    /// True between `start()` and the first tick that consumes it — tells
    /// the loop to seed `filtered_load` and `commanded_position` from the
    /// axis instead of using stale values from a prior run.
    is_first_run: bool,
}

impl Default for PressureControl {
    fn default() -> Self {
        Self {
            state: StateMachine::new(),
            in_tolerance: false,
            target_load: 0.0,
            max_load: f64::INFINITY,
            integral: 0.0,
            prev_error: 0.0,
            filtered_load: 0.0,
            commanded_position: 0.0,
            last_pid_error: 0.0,
            settling_timer: 0.0,
            is_first_run: false,
        }
    }
}

impl PressureControl {
    /// Creates a new, idle PressureControl block.
    pub fn new() -> Self {
        Self::default()
    }

    /// Engage the loop. Latches `target_load` and the safety ceiling
    /// `max_load`; clears PID state. The next `tick()` seeds
    /// `commanded_position` from the axis's current position.
    ///
    /// `max_load` is checked against `|current_load|` every tick; exceeding
    /// it transitions to `Error` and halts the axis. Pass `f64::INFINITY`
    /// if you genuinely want no limit (not recommended).
    pub fn start(&mut self, target_load: f64, max_load: f64) {
        self.state.clear_error();
        self.target_load = target_load;
        self.max_load = max_load;
        self.integral = 0.0;
        self.prev_error = 0.0;
        self.last_pid_error = 0.0;
        self.settling_timer = 0.0;
        self.in_tolerance = false;
        self.is_first_run = true;
        self.state.index = PcState::Controlling as i32;
    }

    /// Halt the axis and transition toward Idle. Safe to call any time;
    /// a no-op when already Idle or Halted.
    pub fn stop(&mut self, axis: &mut impl AxisHandle) {
        let idx = self.state.index;
        if idx == PcState::Idle as i32 || idx == PcState::Halted as i32 {
            return;
        }
        axis.halt();
        self.in_tolerance = false;
        self.state.index = PcState::Halted as i32;
    }

    /// Change the target load without resetting the loop. Useful for
    /// setpoint ramps during a test.
    pub fn set_target(&mut self, target_load: f64) {
        self.target_load = target_load;
    }

    /// Change the max-load safety limit without resetting the loop.
    pub fn set_max_load(&mut self, max_load: f64) {
        self.max_load = max_load;
    }

    /// Current loop state: running or stopping.
    pub fn is_busy(&self) -> bool {
        let idx = self.state.index;
        idx == PcState::Controlling as i32 || idx == PcState::Halted as i32
    }

    /// Fault state. Check `error_message()` for the reason.
    pub fn is_error(&self) -> bool {
        self.state.index == PcState::Error as i32 || self.state.is_error()
    }

    /// Description of the most recent error.
    pub fn error_message(&self) -> String {
        self.state.error_message.clone()
    }

    /// True once the load has been within `config.tolerance` of
    /// `target_load` for at least `config.settling_time` seconds.
    pub fn is_in_tolerance(&self) -> bool {
        self.in_tolerance
    }

    /// Most recent filtered load reading. Updated every `tick()`.
    pub fn filtered_load(&self) -> f64 {
        self.filtered_load
    }

    /// Current internal commanded position (user units). Updated every
    /// `tick()` by adding the clamped PID step.
    pub fn commanded_position(&self) -> f64 {
        self.commanded_position
    }

    /// Most recent PID error: `target_load − filtered_load`. Signed; sign
    /// is NOT inverted by `config.invert_direction`.
    pub fn pid_error(&self) -> f64 {
        self.last_pid_error
    }

    /// Advance the loop by one scan. No-op when Idle.
    pub fn tick(
        &mut self,
        axis: &mut impl AxisHandle,
        current_load: f64,
        config: &PressureControlConfig,
        dt: f64,
    ) {
        // Drive-side fault takes priority over anything the loop is doing.
        if axis.is_error() && self.state.index != PcState::Idle as i32 {
            self.state.set_error(100, "Axis is in error state");
            self.state.index = PcState::Error as i32;
            return;
        }

        match PcState::from_index(self.state.index) {
            Some(PcState::Idle) | Some(PcState::Error) | None => {
                // Idle or errored out — nothing to do. `start()` takes us
                // back to Controlling.
            }

            Some(PcState::Halted) => {
                // `stop()` issued the halt; wait for the drive to finish
                // decelerating before declaring Idle.
                if !axis.is_busy() {
                    self.state.index = PcState::Idle as i32;
                }
            }

            Some(PcState::Controlling) => {
                // 1. Safety first: abort if the raw load exceeded its
                //    ceiling. Must check the raw reading, not the filter,
                //    because a sensor fault can spike faster than the EMA
                //    responds.
                if current_load.abs() > self.max_load {
                    axis.halt();
                    self.state.set_error(
                        110,
                        format!(
                            "load {} exceeded max_load {}",
                            current_load, self.max_load,
                        ),
                    );
                    self.state.index = PcState::Error as i32;
                    return;
                }

                // 2. First-tick seed. Align the filter and commanded
                //    position to current reality so the loop doesn't start
                //    chasing stale values.
                if self.is_first_run {
                    self.filtered_load      = current_load;
                    self.commanded_position = axis.position();
                    self.is_first_run       = false;
                }

                // 3. EMA filter on the load.
                let alpha = config.filter_alpha.clamp(0.0, 1.0);
                self.filtered_load = alpha * current_load
                                   + (1.0 - alpha) * self.filtered_load;

                // 4. PID. Integral + derivative use `dt`; caller owns it.
                let error = self.target_load - self.filtered_load;
                self.last_pid_error = error;

                let derivative = if dt > 0.0 {
                    (error - self.prev_error) / dt
                } else {
                    0.0
                };
                self.prev_error = error;

                // Raw (pre-clamp) output — integral contribution included
                // for now; we'll roll back the integral step if we end up
                // saturating.
                let integral_candidate = (self.integral + error * dt)
                    .clamp(-config.max_integral, config.max_integral);
                let raw_output = config.kp * error
                               + config.ki * integral_candidate
                               + config.kd * derivative
                               + config.feed_forward;

                // 5. Direction + step clamp. Track whether we saturated so
                //    we can apply conditional integration.
                let signed_output = if config.invert_direction {
                    -raw_output
                } else {
                    raw_output
                };
                let saturated = signed_output.abs() > config.max_step;
                let step = signed_output.clamp(-config.max_step, config.max_step);

                // Conditional integration: only commit the integral step
                // when we're NOT bumping against the saturation clamp.
                // Keeps the integrator from winding up during long
                // approaches where max_step is the binding constraint.
                if !saturated {
                    self.integral = integral_candidate;
                }

                // 6. Accumulate commanded position, clamp to optional
                //    envelope. Clamping also freezes the integral this
                //    tick (a second anti-windup belt-and-suspenders).
                let next_cmd = self.commanded_position + step;
                let clamped = match (config.position_limit_neg, config.position_limit_pos) {
                    (Some(lo), Some(hi)) => next_cmd.clamp(lo, hi),
                    (Some(lo), None)     => next_cmd.max(lo),
                    (None, Some(hi))     => next_cmd.min(hi),
                    (None, None)         => next_cmd,
                };
                if clamped != next_cmd {
                    // Clamped — pull the integral back to the pre-accumulate
                    // value so it doesn't keep winding into the wall.
                    self.integral = self.integral - error * dt;
                    self.integral = self.integral.clamp(-config.max_integral, config.max_integral);
                }
                self.commanded_position = clamped;

                // 7. Issue the move, subject to the min-step deadband.
                let issued_step = self.commanded_position - axis.position();
                if issued_step.abs() > config.min_step {
                    let vel = axis.config().jog_speed;
                    let acc = axis.config().jog_accel;
                    let dec = axis.config().jog_decel;
                    axis.move_absolute(self.commanded_position, vel, acc, dec);
                }

                // 8. Tolerance / settling.
                if error.abs() <= config.tolerance {
                    self.settling_timer += dt;
                    if self.settling_timer >= config.settling_time {
                        self.in_tolerance = true;
                    }
                } else {
                    self.settling_timer = 0.0;
                    self.in_tolerance = false;
                }
            }
        }

        self.state.call();
    }
}

// -------------------------------------------------------------------------
// Tests
// -------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::motion::axis_config::AxisConfig;

    struct MockAxis {
        position:        f64,
        busy:            bool,
        error:           bool,
        config:          AxisConfig,
        halt_calls:      u32,
        last_move_target: f64,
        move_calls:      u32,
        /// Simple spring model: position increases force linearly.
        /// current_load = spring_k * (position - rest_position)
        spring_k:        f64,
        rest_position:   f64,
    }

    impl MockAxis {
        fn new() -> Self {
            let mut cfg = AxisConfig::new(10_000);
            cfg.jog_speed = 5.0;
            cfg.jog_accel = 50.0;
            cfg.jog_decel = 50.0;
            Self {
                position: 0.0, busy: false, error: false,
                config: cfg,
                halt_calls: 0, last_move_target: 0.0, move_calls: 0,
                spring_k: 0.0, rest_position: 0.0,
            }
        }
        fn with_spring(mut self, k: f64, rest: f64) -> Self {
            self.spring_k = k;
            self.rest_position = rest;
            self
        }
        fn simulated_load(&self) -> f64 {
            self.spring_k * (self.position - self.rest_position)
        }
        /// Pretend the drive processed the last move_absolute: move the
        /// axis instantly to the commanded position. Tests call this
        /// between ticks to advance the simulation.
        fn advance(&mut self) {
            self.position = self.last_move_target;
        }
    }

    impl AxisHandle for MockAxis {
        fn position(&self) -> f64 { self.position }
        fn config(&self) -> &AxisConfig { &self.config }
        fn move_absolute(&mut self, p: f64, _v: f64, _a: f64, _d: f64) {
            self.last_move_target = p;
            self.move_calls += 1;
            self.busy = true;
        }
        fn move_relative(&mut self, _: f64, _: f64, _: f64, _: f64) {}
        fn halt(&mut self) {
            self.halt_calls += 1;
            self.busy = false;
        }
        fn is_busy(&self) -> bool { self.busy }
        fn is_error(&self) -> bool { self.error }
        fn motor_on(&self) -> bool { true }
    }

    fn zero_pid_config() -> PressureControlConfig {
        PressureControlConfig { ..Default::default() }
    }

    #[test]
    fn tick_is_noop_when_idle() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        let cfg = zero_pid_config();
        pc.tick(&mut axis, 0.0, &cfg, 0.01);
        assert!(!pc.is_busy());
        assert_eq!(axis.move_calls, 0);
        assert_eq!(axis.halt_calls, 0);
    }

    #[test]
    fn start_then_tick_seeds_from_axis() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        axis.position = 3.25;
        pc.start(10.0, 500.0);
        assert!(pc.is_busy());
        // is_first_run catches in this tick.
        pc.tick(&mut axis, 0.0, &zero_pid_config(), 0.01);
        assert_eq!(pc.commanded_position(), 3.25);
        assert_eq!(pc.filtered_load(), 0.0);
    }

    #[test]
    fn stop_halts_and_transitions_to_idle() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        pc.start(10.0, 500.0);
        pc.tick(&mut axis, 5.0, &zero_pid_config(), 0.01);
        assert!(pc.is_busy());
        pc.stop(&mut axis);
        assert_eq!(axis.halt_calls, 1);
        assert!(pc.is_busy(), "still busy until axis finishes halt");
        axis.busy = false;  // drive finished deceleration
        pc.tick(&mut axis, 5.0, &zero_pid_config(), 0.01);
        assert!(!pc.is_busy());
    }

    #[test]
    fn max_load_triggers_error_and_halt() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        pc.start(10.0, 100.0);
        pc.tick(&mut axis, 150.0, &zero_pid_config(), 0.01);
        assert!(pc.is_error());
        assert_eq!(axis.halt_calls, 1);
        assert!(pc.error_message().contains("exceeded max_load"));
    }

    #[test]
    fn loop_converges_toward_target() {
        // Spring model: 100 N per mm starting at rest=0. Target 50 N → 0.5 mm.
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new().with_spring(100.0, 0.0);
        let cfg = PressureControlConfig {
            kp: 0.01,       // 0.01 mm per N of error
            ki: 0.0,
            kd: 0.0,
            max_step: 0.5,
            min_step: 0.0,
            filter_alpha: 1.0,
            tolerance: 1.0,
            settling_time: 0.0,
            ..Default::default()
        };
        pc.start(50.0, 500.0);
        for _ in 0..200 {
            let load = axis.simulated_load();
            pc.tick(&mut axis, load, &cfg, 0.01);
            axis.advance();
        }
        let final_load = axis.simulated_load();
        assert!(
            (final_load - 50.0).abs() < 1.0,
            "expected load near 50 N, got {}", final_load,
        );
        assert!(pc.is_in_tolerance());
    }

    #[test]
    fn invert_direction_flips_step_sign() {
        // Axis where moving NEGATIVE increases compression.
        // spring_k = -100 N/mm, rest_position = 0 → load = -100 * position.
        // Target 50 N requires position = -0.5 mm. Without inversion the
        // raw PID output is positive (target − current > 0 → +output); with
        // inversion that becomes a negative step.
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new().with_spring(-100.0, 0.0);
        let cfg = PressureControlConfig {
            kp: 0.01,
            invert_direction: true,
            max_step: 0.5,
            min_step: 0.0,
            filter_alpha: 1.0,
            tolerance: 1.0,
            settling_time: 0.0,
            ..Default::default()
        };
        pc.start(50.0, 500.0);
        for _ in 0..200 {
            let load = axis.simulated_load();
            pc.tick(&mut axis, load, &cfg, 0.01);
            axis.advance();
        }
        assert!(axis.position < 0.0, "expected negative position, got {}", axis.position);
    }

    #[test]
    fn min_step_deadband_suppresses_move() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        let cfg = PressureControlConfig {
            kp: 0.0001,       // tiny gain → tiny step
            max_step: 0.5,
            min_step: 0.01,   // step 0.0001 * 1 = 0.0001 → well below 0.01
            filter_alpha: 1.0,
            ..Default::default()
        };
        pc.start(1.0, 500.0);
        pc.tick(&mut axis, 0.0, &cfg, 0.01);  // seeds
        let first_move_calls = axis.move_calls;
        // Steady state: step is tiny every tick → no moves.
        for _ in 0..10 {
            pc.tick(&mut axis, 0.0, &cfg, 0.01);
        }
        assert_eq!(axis.move_calls, first_move_calls,
            "min_step should have suppressed all these moves");
    }

    #[test]
    fn position_limit_clamps_commanded() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        let cfg = PressureControlConfig {
            kp: 1.0,
            max_step: 0.5,
            min_step: 0.0,
            filter_alpha: 1.0,
            position_limit_pos: Some(1.0),  // Cap at 1 mm
            tolerance: 1.0,
            settling_time: 0.0,
            ..Default::default()
        };
        pc.start(100.0, 500.0);  // huge error → will try to run away
        for _ in 0..20 {
            pc.tick(&mut axis, 0.0, &cfg, 0.01);
        }
        assert!(pc.commanded_position() <= 1.0 + 1e-6,
            "commanded position should be clamped, got {}", pc.commanded_position());
    }

    #[test]
    fn axis_error_transitions_to_error_state() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new();
        pc.start(10.0, 500.0);
        pc.tick(&mut axis, 5.0, &zero_pid_config(), 0.01);
        assert!(pc.is_busy());
        axis.error = true;
        pc.tick(&mut axis, 5.0, &zero_pid_config(), 0.01);
        assert!(pc.is_error());
    }

    #[test]
    fn set_target_changes_setpoint_without_reset() {
        let mut pc = PressureControl::new();
        let mut axis = MockAxis::new().with_spring(100.0, 0.0);
        let cfg = PressureControlConfig {
            kp: 0.01, max_step: 0.5, min_step: 0.0, filter_alpha: 1.0,
            tolerance: 0.5, settling_time: 0.0, ..Default::default()
        };
        pc.start(50.0, 500.0);
        for _ in 0..200 {
            let load = axis.simulated_load();
            pc.tick(&mut axis, load, &cfg, 0.01);
            axis.advance();
        }
        assert!((axis.simulated_load() - 50.0).abs() < 1.0);
        pc.set_target(30.0);
        for _ in 0..200 {
            let load = axis.simulated_load();
            pc.tick(&mut axis, load, &cfg, 0.01);
            axis.advance();
        }
        assert!((axis.simulated_load() - 30.0).abs() < 1.0);
    }
}