autocore-std 3.3.56

//! Axis configuration: unit conversion, jog defaults, limits.
//!
//! [`AxisConfig`] stores the encoder resolution and user-unit scaling,
//! plus jog parameters, home position, and software position limits.
//! It is used by [`Axis`](super::Axis) for all unit conversions.

/// Configuration for a single motion axis.
///
/// Stores encoder resolution, user-unit scaling, jog defaults,
/// and position limits. Used by [`Axis`](super::Axis) internally
/// and also available for direct use (e.g. logging, HMI display).
///
/// # Examples
///
/// ```
/// use autocore_std::motion::AxisConfig;
///
/// // ClearPath with 12,800 counts/rev, user units = degrees
/// let cfg = AxisConfig::new(12_800).with_user_scale(360.0);
/// assert!((cfg.to_counts(45.0) - 1600.0).abs() < 0.01);
/// assert!((cfg.to_user(1600.0) - 45.0).abs() < 0.01);
///
/// // mm on a 5 mm/rev ballscrew
/// let mm = AxisConfig::new(12_800).with_user_scale(5.0);
/// assert!((mm.to_counts(5.0) - 12800.0).abs() < 0.01);
/// ```
#[derive(Debug, Clone)]
pub struct AxisConfig {
    // ── Unit conversion (private — use to_counts/to_user) ──
    counts_per_rev: f64,
    user_per_rev: f64,
    /// When true, invert the sign of all position/velocity conversions.
    /// Use when the motor counts in the opposite direction to your user-unit convention.
    pub invert_direction: bool,

    // ── Jog defaults ──

    /// Jog speed in user units/s.
    pub jog_speed: f64,
    /// Jog acceleration in user units/s².
    pub jog_accel: f64,
    /// Jog deceleration in user units/s².
    pub jog_decel: f64,

    // ── Home ──

    /// User-unit position assigned at the home reference point.
    pub home_position: f64,
    /// Homing search speed in user units/s.
    pub homing_speed: f64,
    /// Homing acceleration in user units/s².
    pub homing_accel: f64,
    /// Homing deceleration in user units/s².
    pub homing_decel: f64,
    /// CiA 402 homing method used when the axis needs to declare "current position
    /// = home offset" — i.e. the terminal step of software homing (after a sensor
    /// search) and the integrated [`HomingMethod::CurrentPosition`] variant.
    ///
    /// Methods 37 and 35 are functionally equivalent; 37 is the newer CiA 402
    /// addition. Default: 37 (works on Teknic ClearPath and other modern drives).
    /// Set to 35 for drives whose 6098h range stops at 35 (Inovance SV660N).
    pub soft_home_method: i8,

    /// Drive trait: does this drive refuse to assert set-point-acknowledge
    /// (status word bit 12) while Halt (control word bit 8) is asserted?
    ///
    /// In CiA-402 Profile Position mode the cancel handshake that closes out a
    /// halt issues a new set-point (current position, zero velocity) and waits
    /// for the drive to acknowledge it. Drives disagree on whether that
    /// acknowledge is produced while Halt is still active — and this is the
    /// one behavioral split between the drives we support:
    ///
    /// - **`true`** — the drive will NOT acknowledge a set-point while halted,
    ///   so [`command_cancel_move`](super::axis::Axis::command_cancel_move)
    ///   must clear the halt bit while issuing the cancel set-point. The
    ///   Kollmorgen AKD and Inovance SV660N both behave this way. Two
    ///   independent, reference-grade CiA-402 implementations agreeing makes
    ///   this the *standard* behavior.
    /// - **`false` (default)** — halt is held through the whole cancel
    ///   handshake. The Teknic ClearPath needs this: it acknowledges the
    ///   set-point while halted, and clearing halt early makes it resume the
    ///   prior move. Teknic is the oddball here.
    ///
    /// The default is `false` deliberately as the **safe fallback**, not
    /// because it is the common case: if an unknown drive actually needs halt
    /// cleared, the halt merely times out with the motor already stopped
    /// (loud and safe), whereas defaulting to `true` on a Teknic-like drive
    /// could provoke unexpected motion when halt drops mid-handshake.
    ///
    /// Typically not hand-set: the drive's extended-info default is seeded into
    /// the axis's `options` in `project.json` (overridable there) and baked into
    /// the generated drive handle by codegen, the same path as `invert_direction`.
    pub halt_blocks_setpoint_ack: bool,

    // ── Timeouts ──

    /// Timeout for general operations (enable, disable, halt, fault recovery) in seconds.
    /// Default: 7s.
    pub operation_timeout_secs: f64,
    /// Timeout for homing operations in seconds. Default: 30s.
    pub homing_timeout_secs: f64,

    // ── Software position limits ──

    /// Enable the maximum (positive) software position limit.
    pub enable_max_position_limit: bool,
    /// Enable the minimum (negative) software position limit.
    pub enable_min_position_limit: bool,
    /// Maximum position limit in user units.
    pub max_position_limit: f64,
    /// Minimum position limit in user units.
    pub min_position_limit: f64,
}

impl AxisConfig {
    /// Create a new configuration from encoder resolution.
    ///
    /// Default user units are revolutions (1.0 user unit = 1 revolution).
    /// Call [`with_user_scale`](Self::with_user_scale) to change.
    pub fn new(counts_per_rev: u32) -> Self {
        Self {
            counts_per_rev: counts_per_rev as f64,
            user_per_rev: 1.0,
            invert_direction: false,
            jog_speed: 0.0,
            jog_accel: 0.0,
            jog_decel: 0.0,
            home_position: 0.0,
            homing_speed: 0.0,
            homing_accel: 0.0,
            homing_decel: 0.0,
            soft_home_method: 37,
            halt_blocks_setpoint_ack: false,
            operation_timeout_secs: 7.0,
            homing_timeout_secs: 30.0,
            enable_max_position_limit: false,
            enable_min_position_limit: false,
            max_position_limit: 0.0,
            min_position_limit: 0.0,
        }
    }

    /// Set user-units-per-revolution.
    ///
    /// - Degrees: `.with_user_scale(360.0)`
    /// - mm on 5 mm/rev ballscrew: `.with_user_scale(5.0)`
    pub fn with_user_scale(mut self, user_per_rev: f64) -> Self {
        self.user_per_rev = user_per_rev;
        self
    }

    // ── Conversion methods ──

    /// Convert user units to encoder counts (f64).
    ///
    /// `to_counts(45.0)` on a 12800 cpr / 360° config → 1600.0
    pub fn to_counts(&self, user_units: f64) -> f64 {
        let sign = if self.invert_direction { -1.0 } else { 1.0 };
        user_units * self.counts_per_user() * sign
    }

    /// Convert encoder counts to user units (f64).
    ///
    /// `to_user(1600.0)` on a 12800 cpr / 360° config → 45.0
    pub fn to_user(&self, counts: f64) -> f64 {
        let sign = if self.invert_direction { -1.0 } else { 1.0 };
        counts * sign / self.counts_per_user()
    }

    /// Encoder counts per user unit (scale factor).
    ///
    /// For 12800 cpr / 360°: `counts_per_user() ≈ 35.556`
    pub fn counts_per_user(&self) -> f64 {
        self.counts_per_rev / self.user_per_rev
    }
}

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

    #[test]
    fn halt_blocks_setpoint_ack_defaults_to_safe_false() {
        // The safe fallback: an unknown drive holds halt through the cancel
        // handshake (Teknic-style) and fails loud rather than risking motion.
        let cfg = AxisConfig::new(1_048_576);
        assert!(!cfg.halt_blocks_setpoint_ack);
    }

    #[test]
    fn basic_conversion_degrees() {
        let cfg = AxisConfig::new(12_800).with_user_scale(360.0);
        assert!((cfg.to_counts(45.0) - 1600.0).abs() < 0.01);
        assert!((cfg.to_counts(360.0) - 12800.0).abs() < 0.01);
        assert!((cfg.to_user(1600.0) - 45.0).abs() < 0.01);
        assert!((cfg.to_user(12800.0) - 360.0).abs() < 0.01);
    }

    #[test]
    fn basic_conversion_mm() {
        let cfg = AxisConfig::new(12_800).with_user_scale(5.0);
        assert!((cfg.to_counts(5.0) - 12800.0).abs() < 0.01);
        assert!((cfg.to_counts(2.5) - 6400.0).abs() < 0.01);
    }

    #[test]
    fn counts_per_user() {
        let cfg = AxisConfig::new(12_800).with_user_scale(360.0);
        let cpu = cfg.counts_per_user();
        assert!((cpu - 12800.0 / 360.0).abs() < 1e-9);
    }

    #[test]
    fn default_revolutions() {
        let cfg = AxisConfig::new(12_800);
        // 1 rev = 12800 counts
        assert!((cfg.to_counts(1.0) - 12800.0).abs() < 0.01);
        assert!((cfg.to_counts(0.125) - 1600.0).abs() < 0.01);
    }

    #[test]
    fn round_trip() {
        let cfg = AxisConfig::new(12_800).with_user_scale(360.0);
        let original = 45.0;
        let counts = cfg.to_counts(original);
        let back = cfg.to_user(counts);
        assert!((back - original).abs() < 1e-9);
    }

    #[test]
    fn defaults() {
        let cfg = AxisConfig::new(12_800);
        assert_eq!(cfg.jog_speed, 0.0);
        assert_eq!(cfg.home_position, 0.0);
        assert!(!cfg.enable_max_position_limit);
        assert!(!cfg.enable_min_position_limit);
    }
}