servo_pio/
servo_state.rs

1use crate::calibration::{self, Calibration, CalibrationData, Point};
2
3pub const DEFAULT_FREQUENCY: f32 = 50.0;
4pub const MIN_FREQUENCY: f32 = 10.0;
5pub const MAX_FREQUENCY: f32 = 350.0;
6const MIN_VALID_PULSE: f32 = 1.0;
7
8/// Type to manage state for a single servo.
9pub struct ServoState<C> {
10    /// The current value for the servo. The unit is derived from the calibration.
11    servo_value: f32,
12    /// The most recent pulse value for this servo while it was enabled.
13    last_enabled_pulse: Option<f32>,
14    /// Whether or not the servo is enabled.
15    enabled: bool,
16    /// The calibration for this servo.
17    calibration: Calibration<C>,
18}
19
20impl<C> ServoState<C>
21where
22    C: Default + Clone + CalibrationData,
23{
24    /// Constructs a new `ServoState` if the calibration type has 2 or more calibration points.
25    pub fn new() -> Option<Self> {
26        Some(Self {
27            servo_value: 0.0,
28            last_enabled_pulse: None,
29            enabled: false,
30            calibration: Calibration::new()?,
31        })
32    }
33}
34
35impl<C> ServoState<C>
36where
37    C: CalibrationData,
38    for<'a> <C as CalibrationData>::Iterator<'a>: Iterator<Item = (Point, Point)>,
39{
40    /// Construct a `ServoState` based on some [Calibration].
41    pub fn with_calibration(calibration: Calibration<C>) -> Self {
42        Self {
43            servo_value: 0.0,
44            last_enabled_pulse: None,
45            enabled: false,
46            calibration,
47        }
48    }
49
50    /// Enable the servo and return its pulse.
51    pub fn enable_with_return(&mut self) -> f32 {
52        if let Some(pulse) = self.last_enabled_pulse {
53            self.enabled = true;
54            pulse
55        } else {
56            self.go_to_mid_with_return()
57        }
58    }
59
60    /// Set the value to the mid value and return the pulse.
61    #[inline]
62    fn go_to_mid_with_return(&mut self) -> f32 {
63        self.set_value_with_return(self.calibration.mid_value())
64    }
65
66    /// # Panic Safety
67    /// Caller ensures last_enabled_pulse is occupied.
68    #[inline]
69    fn inner_enable_with_return(&mut self) -> Option<f32> {
70        self.enabled = true;
71        self.last_enabled_pulse
72    }
73
74    /// Set the value and return the pulse.
75    pub fn set_value_with_return(&mut self, value: f32) -> f32 {
76        let point = self.calibration.value_to_pulse(value);
77        self.last_enabled_pulse = Some(point.pulse);
78        self.servo_value = point.value;
79        self.enabled = true;
80        point.pulse
81    }
82
83    /// Disable the servo.
84    #[inline]
85    pub fn disable(&mut self) {
86        self.enabled = false;
87    }
88
89    /// Return whether the servo is enabled.
90    #[inline]
91    pub fn enabled(&self) -> bool {
92        self.enabled
93    }
94
95    /// Convert the pulse, given a resolution and frequency, into a level for use by pwm signals.
96    pub fn pulse_to_level(pulse: f32, resolution: u32, frequency: f32) -> u32 {
97        if pulse >= MIN_VALID_PULSE {
98            ((pulse * resolution as f32 * frequency) as u64 / 1_000_000) as u32
99        } else {
100            0
101        }
102    }
103
104    /// Get the pulse if available.
105    #[inline]
106    pub fn pulse(&self) -> Option<f32> {
107        self.last_enabled_pulse
108    }
109
110    /// Set the pulse and return the new pulse value. Can return `None` when pulse is not larger
111    /// than the minimum valid pulse.
112    pub fn set_pulse_with_return(&mut self, pulse: f32) -> Option<f32> {
113        if pulse >= MIN_VALID_PULSE {
114            if let Some(point) = self.calibration.pulse_to_value(pulse) {
115                self.servo_value = point.value;
116                self.last_enabled_pulse = Some(point.pulse);
117                return self.inner_enable_with_return();
118            }
119        }
120        self.disable();
121        None
122    }
123
124    /// Get the current value of the servo.
125    #[inline]
126    pub fn value(&self) -> f32 {
127        self.servo_value
128    }
129
130    /// Get the minimum value the servo can move to.
131    #[inline]
132    pub(crate) fn min_value(&self) -> f32 {
133        self.calibration.first().value
134    }
135
136    /// Get the mid-point value the servo can move to.
137    #[inline]
138    pub(crate) fn mid_value(&self) -> f32 {
139        self.calibration.mid_value()
140    }
141
142    /// Get the max value the servo can move to.
143    #[inline]
144    pub(crate) fn max_value(&self) -> f32 {
145        self.calibration.last().value
146    }
147
148    /// Move the servo to the minimum value and return the pulse.
149    #[inline]
150    pub fn to_min_with_return(&mut self) -> f32 {
151        self.set_value_with_return(self.min_value())
152    }
153
154    /// Move the servo to the mid-point value and return the pulse.
155    #[inline]
156    pub fn to_mid_with_return(&mut self) -> f32 {
157        self.set_value_with_return(self.mid_value())
158    }
159
160    /// Move the servo to the maximum value and return the pulse.
161    #[inline]
162    pub fn to_max_with_return(&mut self) -> f32 {
163        self.set_value_with_return(self.max_value())
164    }
165
166    /// Move the servo to a percentage of its movement range.
167    ///
168    /// 0% corresponds to the servo's minimum value, and 100% corresponds to the servo's maximum value.
169    pub fn to_percent_with_return(&mut self, percent: f32) -> f32 {
170        let value = calibration::map_float(percent, 0.0, 100.0, self.min_value(), self.max_value());
171        self.set_value_with_return(value)
172    }
173
174    /// Get a shared reference of the servo's calibration.
175    #[inline]
176    pub fn calibration(&self) -> &Calibration<C> {
177        &self.calibration
178    }
179
180    /// Get a unique reference to the servos' calibration.
181    #[inline]
182    pub fn calibration_mut(&mut self) -> &mut Calibration<C> {
183        &mut self.calibration
184    }
185}
servo_pio/servo_state.rs

servo_pio/
servo_state.rs
