autocore_std/motion/

sim.rs

//! Virtual (simulated) CiA-402 drive backing a fieldbus-less axis.
//!
//! [`SimDrive`] implements [`AxisView`](super::AxisView) — the same seam an
//! EtherCAT drive handle implements — so the generic [`Axis`](super::Axis)
//! controller drives it with zero changes. It emulates a Profile Position
//! drive: the CiA-402 state machine (Switch On Disabled → … → Operation
//! Enabled), the set-point-acknowledge handshake, and motion integration.
//!
//! Because there is no bus, the runtime must advance the simulation each
//! control cycle by calling [`SimDrive::step`] (analogous to the EtherCAT
//! handle's PDO exchange) before the axis reads feedback.
//!
//! ```
//! use autocore_std::motion::sim::SimDrive;
//! use autocore_std::motion::{AxisView, cia402::{Cia402Control, RawControlWord}};
//!
//! let mut sim = SimDrive::new();
//! // Command Operation Enabled (CW 0x000F) and step the sim a few cycles.
//! let mut cw = RawControlWord(0); cw.cmd_enable_operation();
//! sim.set_control_word(cw.raw());
//! sim.set_modes_of_operation(1); // PP
//! for _ in 0..3 { sim.step(0.01); }
//! // Status word bit 2 = Operation Enabled.
//! assert!(sim.status_word() & (1 << 2) != 0);
//! ```

use super::axis_view::AxisView;
use super::cia402::Cia402State;

/// Status-word base pattern for a CiA-402 state (bits 0–6), matching the
/// decode in [`Cia402Status::state`](super::cia402::Cia402Status::state).
fn status_base(state: Cia402State) -> u16 {
    match state {
        Cia402State::NotReadyToSwitchOn  => 0x0000,
        Cia402State::SwitchOnDisabled    => 0x0040,
        Cia402State::ReadyToSwitchOn     => 0x0021,
        Cia402State::SwitchedOn          => 0x0023,
        Cia402State::OperationEnabled    => 0x0027,
        Cia402State::QuickStopActive     => 0x0007,
        Cia402State::FaultReactionActive => 0x000F,
        Cia402State::Fault               => 0x0008,
        Cia402State::Unknown             => 0x0000,
    }
}

/// A simulated CiA-402 Profile Position drive.
///
/// Commands arrive through the [`AxisView`] setters; [`step`](Self::step)
/// advances the state machine and integrates motion; the [`AxisView`] getters
/// report the simulated feedback. Raw drive units throughout (encoder counts,
/// counts/s) — the same units a real PDO carries.
#[derive(Debug, Clone)]
pub struct SimDrive {
    // ── Commanded (written via AxisView) ──
    control_word: u16,
    target_position: i32,
    profile_velocity: u32,
    profile_acceleration: u32,
    profile_deceleration: u32,
    modes_of_operation: i8,

    // ── Simulated feedback ──
    status_word: u16,
    position_actual: i32,
    velocity_actual: i32,
    modes_display: i8,

    // ── Internal sim state ──
    state: Cia402State,
    active_target: i32,
    moving: bool,
    setpoint_ack: bool,
    prev_new_setpoint: bool,

    /// When `true`, the sim refuses to assert set-point-acknowledge while
    /// Halt (CW bit 8) is asserted — emulating the Kollmorgen-AKD / Inovance
    /// behavior so the halt cancel handshake can be exercised in tests.
    /// Default `false` (ideal drive that acks while halted).
    halt_blocks_setpoint_ack: bool,
}

impl Default for SimDrive {
    fn default() -> Self {
        Self {
            control_word: 0,
            target_position: 0,
            profile_velocity: 0,
            profile_acceleration: 0,
            profile_deceleration: 0,
            modes_of_operation: 0,
            status_word: status_base(Cia402State::SwitchOnDisabled) | (1 << 9),
            position_actual: 0,
            velocity_actual: 0,
            modes_display: 0,
            state: Cia402State::SwitchOnDisabled,
            active_target: 0,
            moving: false,
            setpoint_ack: false,
            prev_new_setpoint: false,
            halt_blocks_setpoint_ack: false,
        }
    }
}

impl SimDrive {
    /// New sim drive in `Switch On Disabled`, ready to be enabled.
    pub fn new() -> Self {
        Self::default()
    }

    /// Enable emulation of a drive that won't acknowledge a set-point while
    /// halted (AKD / Inovance). Lets tests exercise the halt cancel handshake
    /// against the awkward case. Default off.
    pub fn set_halt_blocks_setpoint_ack(&mut self, v: bool) {
        self.halt_blocks_setpoint_ack = v;
    }

    /// Advance the simulation by `dt` seconds: run the CiA-402 state machine,
    /// the PP set-point handshake, and integrate motion. Call once per control
    /// cycle before the axis reads feedback.
    pub fn step(&mut self, dt: f64) {
        self.step_state_machine();

        let cw = self.control_word;
        let bit = |n: u8| cw & (1 << n) != 0;
        let new_setpoint = bit(4);
        let halt = bit(8);

        let enabled = self.state == Cia402State::OperationEnabled;
        let pp_mode = self.modes_of_operation == 1;

        // ── Set-point acknowledge handshake (PP) ──
        if enabled && pp_mode {
            if new_setpoint && !self.prev_new_setpoint {
                // Rising edge: latch the target and ack — unless we're emulating
                // a drive that won't ack while halted.
                if !(self.halt_blocks_setpoint_ack && halt) {
                    self.active_target = self.target_position;
                    self.setpoint_ack = true;
                    self.moving = true;
                }
            }
            if !new_setpoint {
                // Master dropped the new-setpoint bit → drop the ack.
                self.setpoint_ack = false;
            }
        } else {
            self.setpoint_ack = false;
            self.moving = false;
        }
        self.prev_new_setpoint = new_setpoint;

        // ── Motion integration ──
        if enabled && pp_mode && !halt && self.moving {
            let remaining = self.active_target - self.position_actual;
            if remaining == 0 {
                self.velocity_actual = 0;
                self.moving = false;
            } else {
                let step = ((self.profile_velocity as f64 * dt).round() as i32).max(1);
                if step >= remaining.abs() {
                    self.position_actual = self.active_target;
                    self.velocity_actual = 0;
                    self.moving = false;
                } else {
                    let dir = remaining.signum();
                    self.position_actual += dir * step;
                    self.velocity_actual = dir * self.profile_velocity as i32;
                }
            }
        } else {
            // Halted, disabled, or idle → decelerate immediately to rest.
            self.velocity_actual = 0;
            if halt {
                self.moving = false;
            }
        }

        // ── Assemble the status word ──
        let mut sw = status_base(self.state) | (1 << 9); // bit 9: remote
        if self.setpoint_ack {
            sw |= 1 << 12; // set-point acknowledge
        }
        if !self.moving && self.position_actual == self.active_target {
            sw |= 1 << 10; // target reached
        }
        self.status_word = sw;
        self.modes_display = self.modes_of_operation;
    }

    /// Run the CiA-402 control state machine for one step based on the
    /// control-word command bits (0 Switch On, 1 Enable Voltage, 2 Quick Stop,
    /// 3 Enable Operation). Fault states aren't simulated — the sim never
    /// faults on its own.
    fn step_state_machine(&mut self) {
        let cw = self.control_word;
        let switch_on = cw & (1 << 0) != 0;
        let enable_voltage = cw & (1 << 1) != 0;
        let quick_stop = cw & (1 << 2) != 0;
        let enable_op = cw & (1 << 3) != 0;

        use Cia402State::*;
        // Voltage/quick-stop removed → straight to Switch On Disabled from anywhere.
        if !enable_voltage || !quick_stop {
            self.state = SwitchOnDisabled;
            return;
        }
        // enable_voltage && quick_stop held; advance toward the commanded state.
        self.state = if switch_on {
            if enable_op { OperationEnabled } else { SwitchedOn }
        } else {
            ReadyToSwitchOn
        };
    }
}

impl AxisView for SimDrive {
    fn control_word(&self) -> u16 {
        self.control_word
    }
    fn set_control_word(&mut self, word: u16) {
        self.control_word = word;
    }
    fn set_target_position(&mut self, pos: i32) {
        self.target_position = pos;
    }
    fn set_profile_velocity(&mut self, vel: u32) {
        self.profile_velocity = vel;
    }
    fn set_profile_acceleration(&mut self, accel: u32) {
        self.profile_acceleration = accel;
    }
    fn set_profile_deceleration(&mut self, decel: u32) {
        self.profile_deceleration = decel;
    }
    fn set_modes_of_operation(&mut self, mode: i8) {
        self.modes_of_operation = mode;
    }
    fn modes_of_operation_display(&self) -> i8 {
        self.modes_display
    }
    fn status_word(&self) -> u16 {
        self.status_word
    }
    fn position_actual(&self) -> i32 {
        self.position_actual
    }
    fn velocity_actual(&self) -> i32 {
        self.velocity_actual
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::motion::cia402::{Cia402Control, Cia402Status, RawControlWord, RawStatusWord};

    /// Drive the sim's control word the way the CiA-402 enable sequence would,
    /// stepping a few cycles, and confirm it reaches Operation Enabled.
    fn enable(sim: &mut SimDrive) {
        let mut cw = RawControlWord(0);
        cw.cmd_enable_operation(); // 0x000F
        sim.set_control_word(cw.raw());
        sim.set_modes_of_operation(1); // PP
        for _ in 0..3 {
            sim.step(0.01);
        }
    }

    #[test]
    fn reaches_operation_enabled() {
        let mut sim = SimDrive::new();
        assert_eq!(RawStatusWord(sim.status_word()).state(), Cia402State::SwitchOnDisabled);
        enable(&mut sim);
        assert_eq!(RawStatusWord(sim.status_word()).state(), Cia402State::OperationEnabled);
    }

    #[test]
    fn disable_voltage_returns_to_switch_on_disabled() {
        let mut sim = SimDrive::new();
        enable(&mut sim);
        sim.set_control_word(0x0000); // drop enable voltage
        sim.step(0.01);
        assert_eq!(RawStatusWord(sim.status_word()).state(), Cia402State::SwitchOnDisabled);
    }

    /// Full PP move: new-setpoint rising edge → setpoint-ack → motion to target
    /// → target-reached, with the ack dropping when the master clears bit 4.
    #[test]
    fn pp_move_runs_handshake_and_reaches_target() {
        let mut sim = SimDrive::new();
        enable(&mut sim);

        sim.set_target_position(1000);
        sim.set_profile_velocity(10_000); // counts/s → 100 counts per 10 ms step

        // Rising edge on new-setpoint (bit 4).
        let mut cw = RawControlWord(sim.control_word());
        cw.set_bit(4, true);
        sim.set_control_word(cw.raw());
        sim.step(0.01);

        let sw = RawStatusWord(sim.status_word());
        assert!(sw.raw() & (1 << 12) != 0, "setpoint acknowledge asserted on rising edge");
        assert!(sim.velocity_actual() > 0, "moving toward target");

        // Master clears bit 4 → ack drops.
        cw.set_bit(4, false);
        sim.set_control_word(cw.raw());
        sim.step(0.01);
        assert!(RawStatusWord(sim.status_word()).raw() & (1 << 12) == 0, "ack cleared");

        // Run to completion.
        for _ in 0..20 {
            sim.step(0.01);
        }
        assert_eq!(sim.position_actual(), 1000);
        assert_eq!(sim.velocity_actual(), 0);
        assert!(RawStatusWord(sim.status_word()).raw() & (1 << 10) != 0, "target reached");
    }

    #[test]
    fn halt_brings_velocity_to_zero() {
        let mut sim = SimDrive::new();
        enable(&mut sim);
        sim.set_target_position(100_000);
        sim.set_profile_velocity(10_000);
        let mut cw = RawControlWord(sim.control_word());
        cw.set_bit(4, true);
        sim.set_control_word(cw.raw());
        sim.step(0.01);
        assert!(sim.velocity_actual() > 0);

        // Assert halt (bit 8) → velocity goes to zero, position holds.
        cw.set_bit(8, true);
        sim.set_control_word(cw.raw());
        sim.step(0.01);
        assert_eq!(sim.velocity_actual(), 0, "halt stops motion");
    }

    /// With the quirk emulation on, a new-setpoint rising edge issued while
    /// halt is asserted does NOT acknowledge — modeling the AKD/Inovance
    /// behavior the whole halt_blocks_setpoint_ack feature is about.
    #[test]
    fn quirk_emulation_blocks_ack_while_halted() {
        let mut sim = SimDrive::new();
        sim.set_halt_blocks_setpoint_ack(true);
        enable(&mut sim);

        // Halt asserted, then a new-setpoint rising edge (the cancel handshake).
        let mut cw = RawControlWord(sim.control_word());
        cw.set_bit(8, true); // halt
        cw.set_bit(4, true); // new setpoint
        sim.set_control_word(cw.raw());
        sim.step(0.01);
        assert!(RawStatusWord(sim.status_word()).raw() & (1 << 12) == 0,
            "quirk drive must NOT ack a setpoint while halted");

        // Default sim (no quirk) WOULD ack in the same situation.
        let mut ideal = SimDrive::new();
        enable(&mut ideal);
        ideal.set_control_word(cw.raw());
        ideal.step(0.01);
        assert!(RawStatusWord(ideal.status_word()).raw() & (1 << 12) != 0,
            "ideal drive acks even while halted");
    }
}