1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
use crate::{utils::DurationHelpers, Device, Driver, SystemClock};
#[allow(unused_imports)]
use arrayvec::ArrayVec;
use core::time::Duration;
pub struct MultiDriver {
#[cfg(feature = "std")]
drivers: Vec<Driver>,
#[cfg(not(feature = "std"))]
drivers: ArrayVec<[Driver; MultiDriver::MAX_DRIVERS]>,
}
impl MultiDriver {
pub const MAX_DRIVERS: usize = 10;
pub fn new() -> MultiDriver {
MultiDriver {
drivers: Default::default(),
}
}
pub fn push_driver(&mut self, driver: Driver) { self.drivers.push(driver); }
pub fn drivers(&self) -> &[Driver] { &self.drivers }
pub fn drivers_mut(&mut self) -> &mut [Driver] { &mut self.drivers }
pub fn move_to(&mut self, positions: &[i64]) {
assert_eq!(positions.len(), self.drivers.len());
if self.drivers.is_empty() {
return;
}
let longest_time = self
.drivers
.iter()
.zip(positions)
.map(|(d, p)| time_to_move(d, *p))
.max()
.expect("There is always a least one time");
if longest_time == Duration::new(0, 0) {
return;
}
let longest_time = longest_time.as_secs_f32_2();
for (i, driver) in self.drivers.iter_mut().enumerate() {
let distance = positions[i] - driver.current_position();
driver.move_to(positions[i]);
driver.set_speed(distance as f32 / longest_time);
}
}
pub fn poll<D, C>(
&mut self,
devices: &mut [D],
clock: &C,
) -> Result<(), D::Error>
where
D: Device,
C: SystemClock,
{
assert_eq!(devices.len(), self.drivers.len());
for (driver, dev) in self.drivers.iter_mut().zip(devices.iter_mut()) {
driver.poll(dev, clock)?;
}
Ok(())
}
pub fn is_running(&self) -> bool {
self.drivers.iter().any(|d| d.is_running())
}
}
fn time_to_move(driver: &Driver, pos: i64) -> Duration {
let distance = driver.current_position() - pos;
Duration::from_secs_f32_2(distance.abs() as f32 / driver.max_speed())
}