autocore_std/motion/

pp_snapshot.rs

//! Owned-value snapshot of CiA 402 Profile Position (PP) fields.
//!
//! [`Cia402PpSnapshot`] holds all standard CiA 402 PP fields by value
//! (no references, no lifetimes). Used by generated DriveHandle structs
//! to enable multiple axes without borrow conflicts.
//!
//! This is vendor-neutral — it works with any CiA 402 drive (Teknic,
//! Yaskawa, Beckhoff, etc.) as long as the standard PP objects are used.

use super::axis_view::AxisView;
use super::cia402::{
    Cia402Control, Cia402Status, Cia402State,
    PpControl, PpStatus,
    HomingControl, HomingStatus,
    RawControlWord, RawStatusWord,
    ModesOfOperation,
};

// Re-export HomingProgress so generated DriveHandle code can use it
// if needed via autocore_std::motion::HomingProgress
pub use crate::ethercat::teknic::view::HomingProgress;

/// Owned copy of all CiA 402 PP fields for one axis.
///
/// Unlike borrowed views (which hold references into GlobalMemory),
/// this struct owns its data by value. Used by generated DriveHandle
/// structs to enable multiple axes without borrow conflicts.
///
/// Typical flow:
/// 1. `sync()` — copy TxPDO feedback from shared memory into snapshot
/// 2. Issue commands (they modify the snapshot)
/// 3. `tick()` — advance axis state machine, then flush RxPDO back
#[derive(Debug, Clone, Default)]
pub struct Cia402PpSnapshot {
    // ── RxPDO — master → drive ──
    pub control_word: u16,
    pub target_position: i32,
    pub profile_velocity: u32,
    pub profile_acceleration: u32,
    pub profile_deceleration: u32,
    pub modes_of_operation: i8,

    // ── TxPDO — drive → master ──
    pub status_word: u16,
    pub position_actual: i32,
    pub velocity_actual: i32,
    pub torque_actual: i16,
    pub modes_of_operation_display: i8,

    // ���─ Sensor inputs (set from GlobalMemory before tick) ──
    /// Positive-direction limit switch.
    pub positive_limit: bool,
    /// Negative-direction limit switch.
    pub negative_limit: bool,
    /// Home reference sensor.
    pub home_sensor: bool,
    /// Drive error code (vendor-specific, from TxPDO).
    pub error_code: u16,

    /// Dynamic (GM-linked) maximum software position limit in user units.
    /// `None` disables the dynamic limit. Combined with the static config
    /// limit in [`Axis`](super::Axis) using the most-restrictive value.
    pub dynamic_max_position_limit: Option<f64>,
    /// Dynamic (GM-linked) minimum software position limit in user units.
    pub dynamic_min_position_limit: Option<f64>,
}

impl Cia402PpSnapshot {
    // ── Typed access ──

    /// Read the control word as a typed CiA 402 control word.
    pub fn pp_control(&self) -> RawControlWord {
        RawControlWord(self.control_word)
    }

    /// Read the status word as a typed CiA 402 status word.
    pub fn pp_status(&self) -> RawStatusWord {
        RawStatusWord(self.status_word)
    }

    /// Write back a modified control word.
    pub fn set_pp_control(&mut self, cw: RawControlWord) {
        self.control_word = cw.raw();
    }

    // ── State machine ──

    /// Current CiA 402 state.
    pub fn state(&self) -> Cia402State {
        self.pp_status().state()
    }

    /// Shutdown → Ready to Switch On.
    pub fn cmd_shutdown(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_shutdown();
        self.set_pp_control(cw);
    }

    /// Switch On → Switched On.
    pub fn cmd_switch_on(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_switch_on();
        self.set_pp_control(cw);
    }

    /// Enable Operation → Operation Enabled.
    pub fn cmd_enable_operation(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_enable_operation();
        self.set_pp_control(cw);
    }

    /// Disable Operation → Switched On.
    pub fn cmd_disable_operation(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_disable_operation();
        self.set_pp_control(cw);
    }

    /// Disable Voltage → Switch On Disabled.
    pub fn cmd_disable_voltage(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_disable_voltage();
        self.set_pp_control(cw);
    }

    /// Quick Stop → Quick Stop Active.
    pub fn cmd_quick_stop(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_quick_stop();
        self.set_pp_control(cw);
    }

    /// Fault Reset (rising edge on bit 7).
    pub fn cmd_fault_reset(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_fault_reset();
        self.set_pp_control(cw);
    }

    /// Clear/reset Fault Reset bit (bit 7).
    pub fn cmd_clear_fault_reset(&mut self) {
        let mut cw = self.pp_control();
        cw.cmd_clear_fault_reset();
        self.set_pp_control(cw);
    }

    // ── PP motion ──

    /// Set mode to Profile Position.
    pub fn ensure_pp_mode(&mut self) {
        self.modes_of_operation = ModesOfOperation::ProfilePosition.as_i8();
    }

    /// Configure move parameters (does not trigger the move).
    pub fn set_target(&mut self, position: i32, velocity: u32, accel: u32, decel: u32) {
        self.target_position = position;
        self.profile_velocity = velocity;
        self.profile_acceleration = accel;
        self.profile_deceleration = decel;
    }

    /// Assert New Set-Point (bit 4). Call after set_target().
    pub fn trigger_move(&mut self) {
        let mut cw = self.pp_control();
        cw.set_new_set_point(true);
        self.set_pp_control(cw);
    }

    /// Clear New Set-Point. Call when set_point_acknowledged() is true.
    pub fn ack_set_point(&mut self) {
        let mut cw = self.pp_control();
        cw.set_new_set_point(false);
        self.set_pp_control(cw);
    }

    /// Bit 8 — Halt: decelerate to stop.
    pub fn set_halt(&mut self, v: bool) {
        let mut cw = self.pp_control();
        PpControl::set_halt(&mut cw, v);
        self.set_pp_control(cw);
    }

    /// Bit 6 — Relative: when true, target_position is relative to the
    /// current *command* position. When false, target_position is absolute.
    pub fn set_relative(&mut self, v: bool) {
        let mut cw = self.pp_control();
        cw.set_relative(v);
        self.set_pp_control(cw);
    }

    // ── PP status queries ──

    /// Drive has reached the target position (bit 10).
    pub fn target_reached(&self) -> bool {
        self.pp_status().pp_target_reached()
    }

    /// Set-point was acknowledged by the drive (bit 12).
    pub fn set_point_acknowledged(&self) -> bool {
        self.pp_status().set_point_acknowledge()
    }

    // ── Error / fault status ──

    /// Bit 13 — Following Error: position tracking error exceeded the
    /// configured limit. Typically leads to a fault transition.
    pub fn following_error(&self) -> bool {
        self.pp_status().following_error()
    }

    /// Bit 11 — Internal Limit Active: a hardware or software limit
    /// is currently active (e.g. position or velocity limit).
    pub fn internal_limit(&self) -> bool {
        self.pp_status().internal_limit()
    }

    /// Bit 7 — Warning: a non-fatal warning condition exists.
    pub fn warning(&self) -> bool {
        self.pp_status().warning()
    }

    /// True if the drive is in Fault or Fault Reaction Active state.
    pub fn is_faulted(&self) -> bool {
        matches!(self.state(), Cia402State::Fault | Cia402State::FaultReactionActive)
    }

    // ── Homing ──

    /// Set mode to Homing.
    pub fn ensure_homing_mode(&mut self) {
        self.modes_of_operation = ModesOfOperation::Homing.as_i8();
    }

    /// Start the homing procedure (rising edge on bit 4).
    pub fn trigger_homing(&mut self) {
        let mut cw = RawControlWord(self.control_word);
        cw.set_homing_start(true);
        self.control_word = cw.raw();
    }

    /// Clear the homing start bit. Call after homing completes.
    pub fn clear_homing_start(&mut self) {
        let mut cw = RawControlWord(self.control_word);
        cw.set_homing_start(false);
        self.control_word = cw.raw();
    }

    /// Decode the current homing progress from status word bits 10, 12, 13.
    ///
    /// Only meaningful when the drive is in Homing mode.
    pub fn homing_progress(&self) -> HomingProgress {
        let sw = RawStatusWord(self.status_word);
        let attained = sw.homing_attained();
        let reached  = sw.homing_target_reached();
        let error    = sw.homing_error();

        if error {
            HomingProgress::Error
        } else if attained && reached {
            HomingProgress::Complete
        } else if attained {
            HomingProgress::Attained
        } else if reached {
            HomingProgress::Idle
        } else {
            HomingProgress::InProgress
        }
    }

    // ── Feedback ──

    /// Actual position in encoder counts.
    pub fn position(&self) -> i32 {
        self.position_actual
    }

    /// Actual velocity in encoder counts/s.
    pub fn velocity(&self) -> i32 {
        self.velocity_actual
    }

    /// Actual torque in per-mille of rated torque.
    pub fn torque(&self) -> i16 {
        self.torque_actual
    }

    /// Mode the drive is currently operating in.
    pub fn current_mode(&self) -> Option<ModesOfOperation> {
        ModesOfOperation::from_i8(self.modes_of_operation_display)
    }
}

// ──────────────────────────────────────────────
// AxisView implementation
// ──────────────────────────────────────────────

impl AxisView for Cia402PpSnapshot {
    fn control_word(&self) -> u16 { self.control_word }
    fn set_control_word(&mut self, word: u16) { self.control_word = word; }
    fn status_word(&self) -> u16 { self.status_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_of_operation_display }
    fn position_actual(&self) -> i32 { self.position_actual }
    fn velocity_actual(&self) -> i32 { self.velocity_actual }
    fn positive_limit_active(&self) -> bool { self.positive_limit }
    fn negative_limit_active(&self) -> bool { self.negative_limit }
    fn home_sensor_active(&self) -> bool { self.home_sensor }
    fn error_code(&self) -> u16 { self.error_code }
    fn dynamic_max_position_limit(&self) -> Option<f64> { self.dynamic_max_position_limit }
    fn dynamic_min_position_limit(&self) -> Option<f64> { self.dynamic_min_position_limit }
}

// ──────────────────────────────────────────────
// Tests
// ──────────────────────────────────────────────

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

    #[test]
    fn test_snapshot_default() {
        let s = Cia402PpSnapshot::default();
        assert_eq!(s.control_word, 0);
        assert_eq!(s.status_word, 0);
        assert_eq!(s.target_position, 0);
    }

    #[test]
    fn test_snapshot_state_machine() {
        let mut s = Cia402PpSnapshot { status_word: 0x0040, ..Default::default() };
        assert_eq!(s.state(), Cia402State::SwitchOnDisabled);

        s.cmd_shutdown();
        assert_eq!(s.control_word & 0x008F, 0x0006);

        s.cmd_enable_operation();
        assert_eq!(s.control_word & 0x008F, 0x000F);
    }

    #[test]
    fn test_snapshot_pp_motion() {
        let mut s = Cia402PpSnapshot::default();
        s.cmd_enable_operation();
        s.set_target(50_000, 10_000, 2_000, 2_000);
        s.trigger_move();

        assert_eq!(s.target_position, 50_000);
        assert_eq!(s.profile_velocity, 10_000);
        assert!(s.control_word & (1 << 4) != 0);

        s.ack_set_point();
        assert!(s.control_word & (1 << 4) == 0);
    }

    #[test]
    fn test_snapshot_homing() {
        let mut s = Cia402PpSnapshot::default();
        s.ensure_homing_mode();
        assert_eq!(s.modes_of_operation, 6);

        s.cmd_enable_operation();
        s.trigger_homing();
        assert!(s.control_word & (1 << 4) != 0);

        s.clear_homing_start();
        assert!(s.control_word & (1 << 4) == 0);
    }

    #[test]
    fn test_snapshot_homing_progress() {
        // Complete: bit 12 + bit 10
        let s = Cia402PpSnapshot { status_word: 0x1427, ..Default::default() };
        assert_eq!(s.homing_progress(), HomingProgress::Complete);

        // Error: bit 13
        let s = Cia402PpSnapshot { status_word: 0x2027, ..Default::default() };
        assert_eq!(s.homing_progress(), HomingProgress::Error);

        // In progress: no bits
        let s = Cia402PpSnapshot { status_word: 0x0027, ..Default::default() };
        assert_eq!(s.homing_progress(), HomingProgress::InProgress);
    }

    #[test]
    fn test_snapshot_axis_view() {
        let mut s = Cia402PpSnapshot {
            status_word: 0x0027,
            position_actual: 5000,
            velocity_actual: 1000,
            modes_of_operation_display: 1,
            ..Default::default()
        };

        assert_eq!(AxisView::status_word(&s), 0x0027);
        assert_eq!(AxisView::position_actual(&s), 5000);
        assert_eq!(AxisView::velocity_actual(&s), 1000);

        AxisView::set_control_word(&mut s, 0x000F);
        assert_eq!(s.control_word, 0x000F);
        AxisView::set_target_position(&mut s, 10_000);
        assert_eq!(s.target_position, 10_000);
    }

    #[test]
    fn test_snapshot_error_status() {
        let s = Cia402PpSnapshot { status_word: 0x0008, ..Default::default() };
        assert!(s.is_faulted());

        let s = Cia402PpSnapshot { status_word: 0x2027, ..Default::default() };
        assert!(s.following_error());

        let s = Cia402PpSnapshot { status_word: 0x0827, ..Default::default() };
        assert!(s.internal_limit());
    }
}