stepper_motion/motor/
system.rs

1//! Motor system facade for multi-motor configuration.
2//!
3//! Provides a high-level API for managing multiple motors from a single configuration.
4
5use embedded_hal::delay::DelayNs;
6use embedded_hal::digital::OutputPin;
7use heapless::{FnvIndexMap, String};
8
9use crate::config::{MechanicalConstraints, MotorConfig, SystemConfig};
10use crate::error::{ConfigError, Error, Result};
11use crate::motor::state::Idle;
12use crate::motor::{StepperMotor, StepperMotorBuilder};
13use crate::trajectory::TrajectoryRegistry;
14
15/// A facade for managing multiple stepper motors from configuration.
16///
17/// `MotorSystem` provides a high-level API for:
18/// - Creating motors from named configurations
19/// - Accessing motors by name
20/// - Managing trajectory registries
21///
22/// # Example
23///
24/// ```rust,ignore
25/// use stepper_motion::motor::MotorSystem;
26///
27/// let config: SystemConfig = toml::from_str(CONFIG_TOML)?;
28/// let mut system = MotorSystem::from_config(config);
29///
30/// // Register motors with their hardware pins
31/// system.register_motor("x_axis", step_pin_x, dir_pin_x, delay_x)?;
32/// system.register_motor("y_axis", step_pin_y, dir_pin_y, delay_y)?;
33///
34/// // Access motors by name
35/// if let Some(motor) = system.motor("x_axis") {
36///     println!("X position: {}", motor.position_degrees().0);
37/// }
38/// ```
39pub struct MotorSystem {
40    /// The system configuration.
41    config: SystemConfig,
42    /// Trajectory registry for named lookups.
43    registry: TrajectoryRegistry,
44    /// Registered motor names (actual motors are owned by user due to generic types).
45    registered_motors: FnvIndexMap<String<32>, MechanicalConstraints, 8>,
46}
47
48impl MotorSystem {
49    /// Create a new motor system from configuration.
50    ///
51    /// This initializes the trajectory registry but does not create any motors.
52    /// Motors must be registered individually using `register_motor()` or
53    /// created using `build_motor()`.
54    pub fn from_config(config: SystemConfig) -> Self {
55        let registry = TrajectoryRegistry::from_config(&config);
56        Self {
57            config,
58            registry,
59            registered_motors: FnvIndexMap::new(),
60        }
61    }
62
63    /// Get the system configuration.
64    pub fn config(&self) -> &SystemConfig {
65        &self.config
66    }
67
68    /// Get the trajectory registry.
69    pub fn trajectories(&self) -> &TrajectoryRegistry {
70        &self.registry
71    }
72
73    /// Get a motor configuration by name.
74    ///
75    /// Returns `None` if no motor with that name exists in the configuration.
76    pub fn motor_config(&self, name: &str) -> Option<&MotorConfig> {
77        self.config.motor(name)
78    }
79
80    /// Get mechanical constraints for a motor by name.
81    ///
82    /// Returns `None` if no motor with that name exists.
83    pub fn constraints(&self, name: &str) -> Option<MechanicalConstraints> {
84        self.config
85            .motor(name)
86            .map(MechanicalConstraints::from_config)
87    }
88
89    /// Check if a motor name exists in the configuration.
90    pub fn has_motor(&self, name: &str) -> bool {
91        self.config.motor(name).is_some()
92    }
93
94    /// List all configured motor names.
95    pub fn motor_names(&self) -> impl Iterator<Item = &str> {
96        self.config.motor_names()
97    }
98
99    /// Register a motor as active in the system.
100    ///
101    /// This marks the motor as registered and stores its constraints.
102    /// The actual motor instance is returned to the caller.
103    ///
104    /// # Errors
105    ///
106    /// Returns an error if the motor name doesn't exist in the configuration.
107    pub fn register_motor<STEP, DIR, DELAY>(
108        &mut self,
109        name: &str,
110        step_pin: STEP,
111        dir_pin: DIR,
112        delay: DELAY,
113    ) -> Result<StepperMotor<STEP, DIR, DELAY, Idle>>
114    where
115        STEP: OutputPin,
116        DIR: OutputPin,
117        DELAY: DelayNs,
118    {
119        let motor_config = self.config.motor(name).ok_or_else(|| {
120            Error::Config(ConfigError::MotorNotFound(
121                String::try_from(name).unwrap_or_default(),
122            ))
123        })?;
124
125        let constraints = MechanicalConstraints::from_config(motor_config);
126
127        // Store the constraints for this motor
128        let motor_name: String<32> = String::try_from(name).unwrap_or_default();
129        let _ = self.registered_motors.insert(motor_name, constraints);
130
131        // Build and return the motor
132        StepperMotorBuilder::new()
133            .step_pin(step_pin)
134            .dir_pin(dir_pin)
135            .delay(delay)
136            .from_motor_config(motor_config)
137            .build()
138    }
139
140    /// Build a motor from configuration without registering it.
141    ///
142    /// Use this when you need a motor but don't need system-level tracking.
143    ///
144    /// # Errors
145    ///
146    /// Returns an error if the motor name doesn't exist or building fails.
147    pub fn build_motor<STEP, DIR, DELAY>(
148        &self,
149        name: &str,
150        step_pin: STEP,
151        dir_pin: DIR,
152        delay: DELAY,
153    ) -> Result<StepperMotor<STEP, DIR, DELAY, Idle>>
154    where
155        STEP: OutputPin,
156        DIR: OutputPin,
157        DELAY: DelayNs,
158    {
159        let motor_config = self.config.motor(name).ok_or_else(|| {
160            Error::Config(ConfigError::MotorNotFound(
161                String::try_from(name).unwrap_or_default(),
162            ))
163        })?;
164
165        StepperMotorBuilder::new()
166            .step_pin(step_pin)
167            .dir_pin(dir_pin)
168            .delay(delay)
169            .from_motor_config(motor_config)
170            .build()
171    }
172
173    /// Check if a motor has been registered.
174    pub fn is_registered(&self, name: &str) -> bool {
175        self.registered_motors
176            .iter()
177            .any(|(k, _)| k.as_str() == name)
178    }
179
180    /// Get the number of registered motors.
181    pub fn registered_count(&self) -> usize {
182        self.registered_motors.len()
183    }
184
185    /// Get constraints for a registered motor.
186    ///
187    /// Returns `None` if the motor is not registered.
188    pub fn registered_constraints(&self, name: &str) -> Option<&MechanicalConstraints> {
189        self.registered_motors
190            .iter()
191            .find(|(k, _)| k.as_str() == name)
192            .map(|(_, v)| v)
193    }
194
195    /// Get a trajectory by name, with error if not found.
196    ///
197    /// This is a convenience method that delegates to the registry.
198    pub fn trajectory(&self, name: &str) -> Result<&crate::config::TrajectoryConfig> {
199        self.registry.get_or_error(name)
200    }
201
202    /// Get all trajectory names for a specific motor.
203    pub fn trajectories_for_motor<'a>(
204        &'a self,
205        motor_name: &'a str,
206    ) -> impl Iterator<Item = &'a str> + 'a {
207        self.registry
208            .iter()
209            .filter(move |(_, traj)| traj.motor.as_str() == motor_name)
210            .map(|(name, _)| name)
211    }
212}
213
214#[cfg(test)]
215mod tests {
216    use super::*;
217
218    fn test_config() -> SystemConfig {
219        let toml = r#"
220[motors.x_axis]
221name = "X Axis"
222steps_per_revolution = 200
223microsteps = 16
224max_velocity_deg_per_sec = 360.0
225max_acceleration_deg_per_sec2 = 720.0
226
227[motors.y_axis]
228name = "Y Axis"
229steps_per_revolution = 400
230microsteps = 8
231max_velocity_deg_per_sec = 180.0
232max_acceleration_deg_per_sec2 = 360.0
233
234[trajectories.home_x]
235motor = "x_axis"
236target_degrees = 0.0
237velocity_percent = 50
238
239[trajectories.home_y]
240motor = "y_axis"
241target_degrees = 0.0
242velocity_percent = 50
243"#;
244        toml::from_str(toml).unwrap()
245    }
246
247    #[test]
248    fn test_motor_system_creation() {
249        let config = test_config();
250        let system = MotorSystem::from_config(config);
251
252        assert!(system.has_motor("x_axis"));
253        assert!(system.has_motor("y_axis"));
254        assert!(!system.has_motor("z_axis"));
255    }
256
257    #[test]
258    fn test_motor_names() {
259        let config = test_config();
260        let system = MotorSystem::from_config(config);
261
262        let names: Vec<_> = system.motor_names().collect();
263        assert!(names.contains(&"x_axis"));
264        assert!(names.contains(&"y_axis"));
265    }
266
267    #[test]
268    fn test_constraints_lookup() {
269        let config = test_config();
270        let system = MotorSystem::from_config(config);
271
272        let constraints = system.constraints("x_axis").unwrap();
273        // 200 * 16 = 3200 steps/rev
274        assert_eq!(constraints.steps_per_revolution, 3200);
275
276        let constraints = system.constraints("y_axis").unwrap();
277        // 400 * 8 = 3200 steps/rev
278        assert_eq!(constraints.steps_per_revolution, 3200);
279    }
280
281    #[test]
282    fn test_trajectories_for_motor() {
283        let config = test_config();
284        let system = MotorSystem::from_config(config);
285
286        let x_trajectories: Vec<_> = system.trajectories_for_motor("x_axis").collect();
287        assert!(x_trajectories.contains(&"home_x"));
288        assert!(!x_trajectories.contains(&"home_y"));
289
290        let y_trajectories: Vec<_> = system.trajectories_for_motor("y_axis").collect();
291        assert!(y_trajectories.contains(&"home_y"));
292        assert!(!y_trajectories.contains(&"home_x"));
293    }
294
295    #[test]
296    fn test_trajectory_lookup() {
297        let config = test_config();
298        let system = MotorSystem::from_config(config);
299
300        let traj = system.trajectory("home_x");
301        assert!(traj.is_ok());
302        assert_eq!(traj.unwrap().motor.as_str(), "x_axis");
303
304        let traj = system.trajectory("nonexistent");
305        assert!(traj.is_err());
306    }
307}
stepper_motion/motor/system.rs

stepper_motion/motor/
system.rs
