accel_stepper/
multi_driver.rs

1use crate::{utils::DurationHelpers, Device, Driver, SystemClock};
2#[allow(unused_imports)]
3use arrayvec::ArrayVec;
4use core::time::Duration;
5
6/// Controller for moving multiple axes in a coordinated fashion.
7pub struct MultiDriver {
8    #[cfg(feature = "std")]
9    drivers: Vec<Driver>,
10    #[cfg(not(feature = "std"))]
11    drivers: ArrayVec<[Driver; MultiDriver::MAX_DRIVERS]>,
12}
13
14impl MultiDriver {
15    /// The maximum number of [`Driver`]s that a [`MultiDriver`] can manage when
16    /// compiled without the `std` feature.
17    pub const MAX_DRIVERS: usize = 10;
18
19    pub fn new() -> MultiDriver {
20        MultiDriver {
21            drivers: Default::default(),
22        }
23    }
24
25    /// Add a new [`Driver`] to the list of synchronised axes managed by the
26    /// [`MultiDriver`].
27    ///
28    /// # Panics
29    ///
30    /// When compiling without the `std` feature flag, the [`MultiDriver`] can
31    /// only manage up to [`MultiDriver::MAX_DRIVERS`] drivers.
32    pub fn push_driver(&mut self, driver: Driver) { self.drivers.push(driver); }
33
34    pub fn drivers(&self) -> &[Driver] { &self.drivers }
35
36    pub fn drivers_mut(&mut self) -> &mut [Driver] { &mut self.drivers }
37
38    /// Set the target positions of all managed steppers.
39    ///
40    /// # Panics
41    ///
42    /// The number of managed steppers should be the same as the number of
43    /// positions.
44    pub fn move_to(&mut self, positions: &[i64]) {
45        assert_eq!(positions.len(), self.drivers.len());
46
47        if self.drivers.is_empty() {
48            return;
49        }
50
51        // first find the stepper that will take the longest time to move
52        let longest_time = self
53            .drivers
54            .iter()
55            .zip(positions)
56            .map(|(d, p)| time_to_move(d, *p))
57            .max()
58            .expect("There is always a least one time");
59
60        if longest_time == Duration::new(0, 0) {
61            // nothing else needs to be done
62            return;
63        }
64
65        let longest_time = longest_time.as_secs_f32_2();
66
67        // Now work out a new max speed for each stepper so they will all
68        // arrived at the same time of longestTime
69        for (i, driver) in self.drivers.iter_mut().enumerate() {
70            let distance = positions[i] - driver.current_position();
71
72            driver.move_to(positions[i]);
73            driver.set_speed(distance as f32 / longest_time);
74        }
75    }
76
77    /// Poll the underlying [`Driver`]s, emitting steps to the provided
78    /// [`Device`]s when necessary.
79    ///
80    /// # Panics
81    ///
82    /// The number of managed steppers should be the same as the number of
83    /// devices.
84    pub fn poll<D, C>(
85        &mut self,
86        devices: &mut [D],
87        clock: &C,
88    ) -> Result<(), D::Error>
89    where
90        D: Device,
91        C: SystemClock,
92    {
93        assert_eq!(devices.len(), self.drivers.len());
94
95        for (driver, dev) in self.drivers.iter_mut().zip(devices.iter_mut()) {
96            driver.poll(dev, clock)?;
97        }
98
99        Ok(())
100    }
101
102    /// Are any of the managed steppers still running?
103    pub fn is_running(&self) -> bool {
104        self.drivers.iter().any(|d| d.is_running())
105    }
106}
107
108fn time_to_move(driver: &Driver, pos: i64) -> Duration {
109    let distance = driver.current_position() - pos;
110
111    Duration::from_secs_f32_2(distance.abs() as f32 / driver.max_speed())
112}
accel_stepper/multi_driver.rs

accel_stepper/
multi_driver.rs
