stepper_motion/config/
mechanical.rs

1//! Mechanical constraints derived from motor configuration.
2
3use super::limits::StepLimits;
4use super::motor::MotorConfig;
5use super::units::{DegreesPerSec, DegreesPerSecSquared};
6
7/// Derived mechanical parameters computed from motor configuration.
8///
9/// These are computed once at initialization and used for all motion planning.
10#[derive(Debug, Clone)]
11pub struct MechanicalConstraints {
12    /// Total steps per output revolution (steps × microsteps × gear_ratio).
13    pub steps_per_revolution: u32,
14
15    /// Steps per degree of output rotation.
16    pub steps_per_degree: f32,
17
18    /// Maximum velocity in steps per second.
19    pub max_velocity_steps_per_sec: f32,
20
21    /// Maximum acceleration in steps per second squared.
22    pub max_acceleration_steps_per_sec2: f32,
23
24    /// Minimum step interval in nanoseconds (at max velocity).
25    pub min_step_interval_ns: u32,
26
27    /// Soft limits in steps (if configured).
28    pub limits: Option<StepLimits>,
29
30    /// Maximum velocity in degrees per second.
31    pub max_velocity: DegreesPerSec,
32
33    /// Maximum acceleration in degrees per second squared.
34    pub max_acceleration: DegreesPerSecSquared,
35}
36
37impl MechanicalConstraints {
38    /// Compute mechanical constraints from motor configuration.
39    pub fn from_config(config: &MotorConfig) -> Self {
40        // Total steps per output shaft revolution
41        let steps_per_revolution = (config.steps_per_revolution as f32
42            * config.microsteps.value() as f32
43            * config.gear_ratio) as u32;
44
45        // Steps per degree
46        let steps_per_degree = steps_per_revolution as f32 / 360.0;
47
48        // Convert velocity from deg/sec to steps/sec
49        let max_velocity_steps_per_sec = config.max_velocity.0 * steps_per_degree;
50
51        // Convert acceleration from deg/sec² to steps/sec²
52        let max_acceleration_steps_per_sec2 = config.max_acceleration.0 * steps_per_degree;
53
54        // Minimum step interval at max velocity (nanoseconds)
55        let min_step_interval_ns = if max_velocity_steps_per_sec > 0.0 {
56            (1_000_000_000.0 / max_velocity_steps_per_sec) as u32
57        } else {
58            u32::MAX
59        };
60
61        // Convert soft limits to step limits
62        let limits = config
63            .limits
64            .as_ref()
65            .map(|l| StepLimits::from_soft_limits(l, steps_per_degree));
66
67        Self {
68            steps_per_revolution,
69            steps_per_degree,
70            max_velocity_steps_per_sec,
71            max_acceleration_steps_per_sec2,
72            min_step_interval_ns,
73            limits,
74            max_velocity: config.max_velocity,
75            max_acceleration: config.max_acceleration,
76        }
77    }
78
79    /// Convert degrees to steps.
80    #[inline]
81    pub fn degrees_to_steps(&self, degrees: f32) -> i64 {
82        (degrees * self.steps_per_degree) as i64
83    }
84
85    /// Convert steps to degrees.
86    #[inline]
87    pub fn steps_to_degrees(&self, steps: i64) -> f32 {
88        steps as f32 / self.steps_per_degree
89    }
90
91    /// Convert deg/sec to steps/sec.
92    #[inline]
93    pub fn velocity_to_steps(&self, deg_per_sec: f32) -> f32 {
94        deg_per_sec * self.steps_per_degree
95    }
96
97    /// Convert deg/sec² to steps/sec².
98    #[inline]
99    pub fn acceleration_to_steps(&self, deg_per_sec2: f32) -> f32 {
100        deg_per_sec2 * self.steps_per_degree
101    }
102
103    /// Calculate step interval for a given velocity in steps/sec.
104    #[inline]
105    pub fn velocity_to_interval_ns(&self, velocity_steps_per_sec: f32) -> u32 {
106        if velocity_steps_per_sec > 0.0 {
107            (1_000_000_000.0 / velocity_steps_per_sec) as u32
108        } else {
109            u32::MAX
110        }
111    }
112
113    /// Check if a position is within soft limits.
114    pub fn check_limits(&self, steps: i64) -> Option<i64> {
115        match &self.limits {
116            Some(limits) => limits.apply(steps),
117            None => Some(steps), // No limits = always valid
118        }
119    }
120}
121
122#[cfg(test)]
123mod tests {
124    use super::*;
125    use crate::config::units::Microsteps;
126
127    fn make_test_config() -> MotorConfig {
128        MotorConfig {
129            name: heapless::String::try_from("test").unwrap(),
130            steps_per_revolution: 200,
131            microsteps: Microsteps::SIXTEENTH,
132            gear_ratio: 1.0,
133            max_velocity: DegreesPerSec(360.0),
134            max_acceleration: DegreesPerSecSquared(720.0),
135            invert_direction: false,
136            limits: None,
137            backlash_compensation: None,
138        }
139    }
140
141    #[test]
142    fn test_steps_per_revolution() {
143        let config = make_test_config();
144        let constraints = MechanicalConstraints::from_config(&config);
145
146        // 200 * 16 * 1.0 = 3200
147        assert_eq!(constraints.steps_per_revolution, 3200);
148    }
149
150    #[test]
151    fn test_steps_per_degree() {
152        let config = make_test_config();
153        let constraints = MechanicalConstraints::from_config(&config);
154
155        // 3200 / 360 = 8.889
156        assert!((constraints.steps_per_degree - 8.889).abs() < 0.01);
157    }
158
159    #[test]
160    fn test_velocity_conversion() {
161        let config = make_test_config();
162        let constraints = MechanicalConstraints::from_config(&config);
163
164        // 360 deg/sec * 8.889 steps/deg = 3200 steps/sec
165        assert!((constraints.max_velocity_steps_per_sec - 3200.0).abs() < 1.0);
166    }
167}
stepper_motion/config/mechanical.rs

stepper_motion/config/
mechanical.rs
