autocore_std/ethercat/

axis_config.rs

/// Unit conversion for a rotary/linear axis with a known encoder resolution.
///
/// Stores the encoder's counts-per-revolution and an optional user-unit scale
/// (e.g. 360.0 for degrees, 5.0 for mm-per-rev on a ballscrew). All conversion
/// methods are pure arithmetic — no state, no side effects.
///
/// # Examples
///
/// ```
/// use autocore_std::ethercat::AxisConfig;
///
/// // Revolutions (default)
/// let cfg = AxisConfig::new(12_800);
/// assert_eq!(cfg.rev(0.125), 1_600);
/// assert_eq!(cfg.rev(1.0), 12_800);
///
/// // Degrees
/// let deg = AxisConfig::new(12_800).with_user_scale(360.0);
/// assert_eq!(deg.user(45.0), 1_600);
/// assert_eq!(deg.user(360.0), 12_800);
///
/// // mm on a 5 mm/rev ballscrew
/// let mm = AxisConfig::new(12_800).with_user_scale(5.0);
/// assert_eq!(mm.user(5.0), 12_800);
/// assert_eq!(mm.user(2.5), 6_400);
/// ```
#[derive(Debug, Clone, Copy)]
pub struct AxisConfig {
    counts_per_rev: f64,
    /// User-units per revolution (e.g. 360.0 for degrees, 1.0 for revolutions).
    user_per_rev: f64,
}

impl AxisConfig {
    /// Create from encoder resolution (e.g. 12_800 counts/rev).
    /// Default user units = revolutions (1.0 user unit = 1 revolution).
    pub const fn new(counts_per_rev: u32) -> Self {
        Self {
            counts_per_rev: counts_per_rev as f64,
            user_per_rev: 1.0,
        }
    }

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

    // ── Revolution-based conversions (user → counts) ──

    /// Convert revolutions to encoder counts (position).
    pub fn rev(&self, revolutions: f64) -> i32 {
        (revolutions * self.counts_per_rev).round() as i32
    }

    /// Convert rev/s to counts/s (velocity).
    pub fn rps(&self, rev_per_sec: f64) -> u32 {
        (rev_per_sec * self.counts_per_rev).round() as u32
    }

    /// Convert rev/s² to counts/s² (acceleration).
    pub fn rps2(&self, rev_per_sec2: f64) -> u32 {
        (rev_per_sec2 * self.counts_per_rev).round() as u32
    }

    // ── User-unit-based conversions (user → counts) ──

    /// Convert user units to encoder counts (position).
    pub fn user(&self, units: f64) -> i32 {
        (units / self.user_per_rev * self.counts_per_rev).round() as i32
    }

    /// Convert user-units/s to counts/s (velocity).
    pub fn user_per_s(&self, units_per_sec: f64) -> u32 {
        (units_per_sec / self.user_per_rev * self.counts_per_rev).round() as u32
    }

    /// Convert user-units/s² to counts/s² (acceleration).
    pub fn user_per_s2(&self, units_per_sec2: f64) -> u32 {
        (units_per_sec2 / self.user_per_rev * self.counts_per_rev).round() as u32
    }

    // ── From-counts conversions (counts → rev) ──

    /// Convert encoder counts to revolutions.
    pub fn to_rev(&self, counts: i32) -> f64 {
        counts as f64 / self.counts_per_rev
    }

    /// Convert counts/s to rev/s.
    pub fn to_rps(&self, counts_per_sec: u32) -> f64 {
        counts_per_sec as f64 / self.counts_per_rev
    }

    // ── From-counts conversions (counts → user) ──

    /// Convert encoder counts to user units.
    pub fn to_user(&self, counts: i32) -> f64 {
        counts as f64 / self.counts_per_rev * self.user_per_rev
    }

    /// Convert counts/s to user-units/s.
    pub fn to_user_per_s(&self, counts_per_sec: u32) -> f64 {
        counts_per_sec as f64 / self.counts_per_rev * self.user_per_rev
    }
}

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

    const CPR: u32 = 12_800;

    #[test]
    fn rev_position() {
        let cfg = AxisConfig::new(CPR);
        assert_eq!(cfg.rev(0.125), 1_600);
        assert_eq!(cfg.rev(1.0), 12_800);
        assert_eq!(cfg.rev(-0.5), -6_400);
    }

    #[test]
    fn rev_velocity_accel() {
        let cfg = AxisConfig::new(CPR);
        assert_eq!(cfg.rps(1.0), 12_800);
        assert_eq!(cfg.rps2(1.0), 12_800);
    }

    #[test]
    fn user_degrees() {
        let deg = AxisConfig::new(CPR).with_user_scale(360.0);
        assert_eq!(deg.user(45.0), 1_600);
        assert_eq!(deg.user(360.0), 12_800);
        assert_eq!(deg.user(90.0), 3_200);
    }

    #[test]
    fn user_mm_ballscrew() {
        let mm = AxisConfig::new(CPR).with_user_scale(5.0);
        assert_eq!(mm.user(5.0), 12_800);
        assert_eq!(mm.user(2.5), 6_400);
    }

    #[test]
    fn user_velocity_accel() {
        let deg = AxisConfig::new(CPR).with_user_scale(360.0);
        // 90 deg/s = 0.25 rev/s = 3200 counts/s
        assert_eq!(deg.user_per_s(90.0), 3_200);
        // 180 deg/s² = 0.5 rev/s² = 6400 counts/s²
        assert_eq!(deg.user_per_s2(180.0), 6_400);
    }

    #[test]
    fn round_trip_rev() {
        let cfg = AxisConfig::new(CPR);
        let original = 0.125;
        let counts = cfg.rev(original);
        let back = cfg.to_rev(counts);
        assert!((back - original).abs() < 1e-9);
    }

    #[test]
    fn round_trip_user() {
        let deg = AxisConfig::new(CPR).with_user_scale(360.0);
        let original = 45.0;
        let counts = deg.user(original);
        let back = deg.to_user(counts);
        assert!((back - original).abs() < 0.1);
    }

    #[test]
    fn round_trip_velocity() {
        let cfg = AxisConfig::new(CPR);
        let original = 7.8125;
        let counts = cfg.rps(original);
        let back = cfg.to_rps(counts);
        assert!((back - original).abs() < 1e-9);
    }

    #[test]
    fn to_user_per_s() {
        let deg = AxisConfig::new(CPR).with_user_scale(360.0);
        // 3200 counts/s = 90 deg/s
        assert!((deg.to_user_per_s(3_200) - 90.0).abs() < 1e-9);
    }

    #[test]
    fn const_construction() {
        // Verify const fn works at compile time
        const CFG: AxisConfig = AxisConfig::new(12_800).with_user_scale(360.0);
        assert_eq!(CFG.user(45.0), 1_600);
    }
}