autocore_std/motion/

axis_config.rs

//! 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,

    /// Whether `command_cancel_move` should clear the halt bit (CW 8) at the
    /// same time it issues the cancel setpoint.
    ///
    /// Drives differ on this:
    /// - Teknic ClearPath: halt must stay asserted through the full cancel
    ///   handshake. Clearing it early causes the drive to resume the prior
    ///   move when halt later drops. Leave this `false`.
    /// - Inovance SV660N: refuses to set setpoint_ack (SW 12) while halt is
    ///   asserted, so the WaitCancelAck stage stalls. Set this `true`.
    ///
    /// Default: `false` (Teknic-safe).
    pub clear_halt_during_cancel: 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,
            clear_halt_during_cancel: 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 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);
    }
}