servo_pio/
calibration.rs

1use core::mem::MaybeUninit;
2
3pub const DEFAULT_MIN_PULSE: f32 = 500.0;
4pub const DEFAULT_MID_PULSE: f32 = 1500.0;
5pub const DEFAULT_MAX_PULSE: f32 = 2500.0;
6
7const LOWER_HARD_LIMIT: f32 = 400.0;
8const UPPER_HARD_LIMIT: f32 = 2600.0;
9
10/// Simple type to manage corresponding pulse/value pairs.
11#[derive(Copy, Clone)]
12#[cfg_attr(feature = "defmt", derive(defmt::Format))]
13pub struct Point {
14    pub pulse: f32,
15    pub value: f32,
16}
17
18impl Point {
19    fn new() -> Self {
20        Self {
21            pulse: 0.0,
22            value: 0.0,
23        }
24    }
25}
26
27/// A trait to cover any type that can represent calibration data.
28pub trait CalibrationData {
29    const LEN: usize;
30    type Iterator<'a>
31    where
32        Self: 'a;
33    /// The first data point in the calibration data. This might overlap with
34    /// [Self::penultimate].
35    fn first(&self) -> Point;
36
37    /// The second data point in the calibration data. This might overlap with
38    /// [Self::last].
39    fn second(&self) -> Point;
40
41    /// The penultimate (just before last) data point in the calibration data.
42    /// This might overlap with [Self::first].
43    fn penultimate(&self) -> Point;
44
45    /// The last data point in the calibration data. This might overlap with
46    /// [Self::second].
47    fn last(&self) -> Point;
48
49    /// Returns an iterator that walks over the calibration data in a 2-slot
50    /// window. This is equivalent to the [`windows`] method on [slices].
51    ///
52    /// [`windows`]: slice::windows
53    /// [slices]: slice
54    fn windows(&self) -> Self::Iterator<'_>;
55}
56
57/// Empty type for the default case where no custom calibration format is being used.
58#[derive(Default, Copy, Clone)]
59#[cfg_attr(feature = "defmt", derive(defmt::Format))]
60pub struct NoCustom;
61
62impl CalibrationData for NoCustom {
63    const LEN: usize = 0;
64    type Iterator<'a> = Self;
65    fn first(&self) -> Point {
66        Point::new()
67    }
68
69    fn second(&self) -> Point {
70        Point::new()
71    }
72
73    fn penultimate(&self) -> Point {
74        Point::new()
75    }
76
77    fn last(&self) -> Point {
78        Point::new()
79    }
80
81    fn windows(&self) -> Self::Iterator<'_> {
82        NoCustom
83    }
84}
85
86impl Iterator for NoCustom {
87    type Item = (Point, Point);
88
89    fn next(&mut self) -> Option<Self::Item> {
90        None
91    }
92}
93
94/// Calibration data best suited for angular servos.
95#[derive(Copy, Clone)]
96#[cfg_attr(feature = "defmt", derive(defmt::Format))]
97pub struct AngularCalibration {
98    /// The lowest angle the servo can reach.
99    min: Point,
100    /// The mid-point angle for the servo.
101    mid: Point,
102    /// The largest angle the servo can reach.
103    max: Point,
104}
105impl AngularCalibration {
106    pub fn new(min: Point, mid: Point, max: Point) -> Self {
107        Self { min, mid, max }
108    }
109
110    pub fn set_min_pulse(&mut self, pulse: f32) {
111        self.min.pulse = pulse;
112    }
113
114    pub fn min_pulse(&self) -> f32 {
115        self.min.pulse
116    }
117
118    pub fn set_mid_pulse(&mut self, pulse: f32) {
119        self.mid.pulse = pulse;
120    }
121
122    pub fn mid_pulse(&self) -> f32 {
123        self.mid.pulse
124    }
125
126    pub fn set_max_pulse(&mut self, pulse: f32) {
127        self.max.pulse = pulse;
128    }
129
130    pub fn max_pulse(&self) -> f32 {
131        self.max.pulse
132    }
133}
134
135impl Default for AngularCalibration {
136    fn default() -> Self {
137        Self {
138            mid: Point {
139                pulse: DEFAULT_MIN_PULSE,
140                value: -90.0,
141            },
142            min: Point {
143                pulse: DEFAULT_MID_PULSE,
144                value: 0.0,
145            },
146            max: Point {
147                pulse: DEFAULT_MAX_PULSE,
148                value: 90.0,
149            },
150        }
151    }
152}
153
154pub struct AngularCalibrationIter<'a> {
155    calibration: &'a AngularCalibration,
156    count: u8,
157}
158
159impl<'a> AngularCalibrationIter<'a> {
160    fn new(calibration: &'a AngularCalibration) -> Self {
161        Self {
162            calibration,
163            count: 0,
164        }
165    }
166}
167
168impl<'a> Iterator for AngularCalibrationIter<'a> {
169    type Item = (Point, Point);
170
171    fn next(&mut self) -> Option<Self::Item> {
172        let count = self.count;
173        self.count += 1;
174        match count {
175            0 => Some((self.calibration.min, self.calibration.mid)),
176            1 => Some((self.calibration.mid, self.calibration.max)),
177            _ => None,
178        }
179    }
180}
181
182impl CalibrationData for AngularCalibration {
183    const LEN: usize = 3;
184    type Iterator<'a> = AngularCalibrationIter<'a>;
185
186    fn first(&self) -> Point {
187        self.min
188    }
189
190    fn second(&self) -> Point {
191        self.mid
192    }
193
194    fn penultimate(&self) -> Point {
195        self.mid
196    }
197
198    fn last(&self) -> Point {
199        self.max
200    }
201
202    fn windows(&self) -> Self::Iterator<'_> {
203        AngularCalibrationIter::new(self)
204    }
205}
206
207/// Calibration data best suited for linear servos.
208#[cfg_attr(feature = "defmt", derive(defmt::Format))]
209pub struct LinearCalibration {
210    /// The shortest length the servo can move to.
211    min: Point,
212    /// The largest length the servo can move to.
213    max: Point,
214}
215
216impl Default for LinearCalibration {
217    fn default() -> Self {
218        Self {
219            min: Point {
220                pulse: DEFAULT_MIN_PULSE,
221                value: 0.0,
222            },
223            max: Point {
224                pulse: DEFAULT_MAX_PULSE,
225                value: 1.0,
226            },
227        }
228    }
229}
230
231pub struct LinearCalibrationIter<'a> {
232    calibration: &'a LinearCalibration,
233    count: u8,
234}
235
236impl<'a> LinearCalibrationIter<'a> {
237    fn new(calibration: &'a LinearCalibration) -> Self {
238        Self {
239            calibration,
240            count: 0,
241        }
242    }
243}
244
245impl<'a> Iterator for LinearCalibrationIter<'a> {
246    type Item = (Point, Point);
247
248    fn next(&mut self) -> Option<Self::Item> {
249        let count = self.count;
250        self.count += 1;
251        match count {
252            0 => Some((self.calibration.min, self.calibration.max)),
253            _ => None,
254        }
255    }
256}
257
258impl CalibrationData for LinearCalibration {
259    const LEN: usize = 3;
260    type Iterator<'a> = LinearCalibrationIter<'a> where Self: 'a;
261
262    fn first(&self) -> Point {
263        self.min
264    }
265
266    fn second(&self) -> Point {
267        self.max
268    }
269
270    fn penultimate(&self) -> Point {
271        self.min
272    }
273
274    fn last(&self) -> Point {
275        self.max
276    }
277
278    fn windows(&self) -> Self::Iterator<'_> {
279        LinearCalibrationIter::new(self)
280    }
281}
282
283/// Calibration data best suited for contiuous rotation servos.
284#[cfg_attr(feature = "defmt", derive(defmt::Format))]
285pub struct ContinuousCalibration {
286    /// The value representing max-speed negative rotation.
287    min: Point,
288    /// The value representing no rotation.
289    mid: Point,
290    /// The value representing max-speed positive rotation.
291    max: Point,
292}
293
294impl Default for ContinuousCalibration {
295    fn default() -> Self {
296        Self {
297            mid: Point {
298                pulse: DEFAULT_MIN_PULSE,
299                value: -1.0,
300            },
301            min: Point {
302                pulse: DEFAULT_MID_PULSE,
303                value: 0.0,
304            },
305            max: Point {
306                pulse: DEFAULT_MAX_PULSE,
307                value: 1.0,
308            },
309        }
310    }
311}
312
313pub struct ContinuousCalibrationIter<'a> {
314    calibration: &'a ContinuousCalibration,
315    count: u8,
316}
317
318impl<'a> ContinuousCalibrationIter<'a> {
319    fn new(calibration: &'a ContinuousCalibration) -> Self {
320        Self {
321            calibration,
322            count: 0,
323        }
324    }
325}
326
327impl<'a> Iterator for ContinuousCalibrationIter<'a> {
328    type Item = (Point, Point);
329
330    fn next(&mut self) -> Option<Self::Item> {
331        let count = self.count;
332        self.count += 1;
333        match count {
334            0 => Some((self.calibration.min, self.calibration.max)),
335            _ => None,
336        }
337    }
338}
339
340impl CalibrationData for ContinuousCalibration {
341    const LEN: usize = 3;
342    type Iterator<'a> = ContinuousCalibrationIter<'a> where Self: 'a;
343
344    fn first(&self) -> Point {
345        self.min
346    }
347
348    fn second(&self) -> Point {
349        self.mid
350    }
351
352    fn penultimate(&self) -> Point {
353        self.mid
354    }
355
356    fn last(&self) -> Point {
357        self.max
358    }
359
360    fn windows(&self) -> Self::Iterator<'_> {
361        ContinuousCalibrationIter::new(self)
362    }
363}
364
365/// Calibration data for some type implementing [CalibrationData].
366#[cfg_attr(feature = "defmt", derive(defmt::Format))]
367pub struct Calibration<C> {
368    /// The specific calibration data.
369    calibration: C,
370
371    /// Whether or not to limit based on the first calibration point. If true,
372    /// the servo can never be set below `calibration.first()`. Defaults to true.
373    pub limit_lower: bool,
374
375    /// Whether or not to limit based on the last calibration point. If true,
376    /// the servo can never be set above `calibration.last()`. Defaults to true.
377    pub limit_upper: bool,
378}
379
380impl<C> Clone for Calibration<C>
381where
382    C: Clone,
383{
384    fn clone(&self) -> Self {
385        Self {
386            calibration: self.calibration.clone(),
387            limit_lower: self.limit_lower,
388            limit_upper: self.limit_upper,
389        }
390    }
391}
392
393impl<C> Copy for Calibration<C> where C: Copy {}
394
395impl<C> Calibration<C>
396where
397    C: Default + Clone + CalibrationData,
398{
399    /// Returns a new calibration, but only if `C` has 2 or more calibration points.
400    pub fn new() -> Option<Self> {
401        if <C as CalibrationData>::LEN < 2 {
402            return None;
403        }
404
405        Some(Self {
406            calibration: Default::default(),
407            limit_lower: true,
408            limit_upper: true,
409        })
410    }
411}
412
413#[derive(Copy, Clone)]
414pub struct CalibrationBuilder<C> {
415    calibration: C,
416    limit_lower: bool,
417    limit_upper: bool,
418}
419
420impl<C> Calibration<C> {
421    pub const fn builder(calibration: C) -> CalibrationBuilder<C> {
422        CalibrationBuilder {
423            calibration,
424            limit_lower: false,
425            limit_upper: false,
426        }
427    }
428}
429
430impl<C> CalibrationBuilder<C> {
431    pub const fn limit_lower(mut self) -> Self {
432        self.limit_lower = true;
433        self
434    }
435
436    pub const fn limit_upper(mut self) -> Self {
437        self.limit_upper = true;
438        self
439    }
440
441    pub fn build(self) -> Calibration<C> {
442        Calibration {
443            calibration: self.calibration,
444            limit_lower: self.limit_lower,
445            limit_upper: self.limit_upper,
446        }
447    }
448}
449
450impl<C> Calibration<C>
451where
452    C: CalibrationData,
453    for<'a> <C as CalibrationData>::Iterator<'a>: Iterator<Item = (Point, Point)>,
454{
455    pub fn inner_mut(&mut self) -> &mut C {
456        &mut self.calibration
457    }
458
459    pub fn inner(&self) -> &C {
460        &self.calibration
461    }
462
463    pub fn value_to_pulse(&self, value: f32) -> Point {
464        let first = self.calibration.first();
465        let last = self.calibration.last();
466
467        // Is the vale below the bottom-most calibration pair?
468        let mut point = if value < first.value {
469            // Should the value be limited to the calibration or projected below it?
470            if self.limit_lower {
471                first
472            } else {
473                let second = self.calibration.second();
474                Point {
475                    pulse: map_float(value, first.value, second.value, first.pulse, second.pulse),
476                    value,
477                }
478            }
479            // Is the value above the top-most calibration pair?
480        } else if value > last.value {
481            // Should the value be limited to the calibration or be projected above it?
482            if self.limit_upper {
483                last
484            } else {
485                let penultimate = self.calibration.penultimate();
486                Point {
487                    pulse: map_float(
488                        value,
489                        penultimate.value,
490                        last.value,
491                        penultimate.pulse,
492                        last.pulse,
493                    ),
494                    value,
495                }
496            }
497        } else {
498            // The value must be between two calibration points, so iterate through them to find which ones.
499            let mut point = MaybeUninit::<Point>::uninit();
500            for (smaller, larger) in self.calibration.windows() {
501                if value < larger.value {
502                    point.write(Point {
503                        pulse: map_float(
504                            value,
505                            smaller.value,
506                            larger.value,
507                            smaller.pulse,
508                            larger.pulse,
509                        ),
510                        value,
511                    });
512                    break;
513                }
514            }
515            // Safety: We know the value has to be between the limits, so it is guaranteed to be initialized above.
516            unsafe { point.assume_init() }
517        };
518
519        // Clamp the pulse between the hard limits.
520        if point.pulse < LOWER_HARD_LIMIT || point.pulse > UPPER_HARD_LIMIT {
521            point.pulse = point.pulse.clamp(LOWER_HARD_LIMIT, UPPER_HARD_LIMIT);
522
523            // Is the pulse below the bottom-most calibration pair?
524            if point.pulse < first.pulse {
525                let second = self.calibration.second();
526                point.value = map_float(
527                    point.pulse,
528                    first.pulse,
529                    second.pulse,
530                    first.value,
531                    second.value,
532                );
533                // Is the pulse above the top-most calibration pair?
534            } else if point.pulse > last.pulse {
535                let penultimate = self.calibration.penultimate();
536                point.value = map_float(
537                    point.pulse,
538                    penultimate.pulse,
539                    last.pulse,
540                    penultimate.value,
541                    last.value,
542                );
543            } else {
544                // The pulse must be between two calibration points, so iterate through them to find which ones.
545                for (smaller, larger) in self.calibration.windows() {
546                    if point.pulse < larger.pulse {
547                        point.value = map_float(
548                            point.pulse,
549                            smaller.pulse,
550                            larger.pulse,
551                            smaller.value,
552                            larger.value,
553                        );
554                        break;
555                    }
556                }
557            }
558        }
559
560        point
561    }
562
563    pub fn pulse_to_value(&self, pulse: f32) -> Option<Point> {
564        if <C as CalibrationData>::LEN < 2 {
565            return None;
566        }
567
568        // Clamp the pulse between the hard limits
569        let mut pulse_out = pulse.clamp(LOWER_HARD_LIMIT, UPPER_HARD_LIMIT);
570        let mut value_out = 0.0;
571
572        // Is the pulse below the bottom most calibration pair?
573        if pulse_out < self.calibration.first().pulse {
574            // Should the pulse be limited to the calibration or projected below it?
575            if self.limit_lower {
576                value_out = self.calibration.first().value;
577                pulse_out = self.calibration.first().pulse;
578            } else {
579                value_out = map_float(
580                    pulse,
581                    self.calibration.first().pulse,
582                    self.calibration.second().pulse,
583                    self.calibration.first().value,
584                    self.calibration.second().value,
585                );
586            }
587        }
588        // Is the pulse above the top most calibration pair?
589        else if pulse > self.calibration.last().pulse {
590            // Should the pulse be limited to the calibration or projected above it?
591            if self.limit_upper {
592                value_out = self.calibration.last().value;
593                pulse_out = self.calibration.last().pulse;
594            } else {
595                value_out = map_float(
596                    pulse,
597                    self.calibration.penultimate().pulse,
598                    self.calibration.last().pulse,
599                    self.calibration.penultimate().value,
600                    self.calibration.last().value,
601                );
602            }
603        } else {
604            // The pulse must between two calibration pairs, so iterate through them to find which ones
605            for (left, right) in self.calibration.windows() {
606                if pulse < right.pulse {
607                    value_out = map_float(pulse, left.pulse, right.pulse, left.value, right.value);
608                    break; // No need to continue checking so break out of the loop
609                }
610            }
611        }
612
613        Some(Point {
614            value: value_out,
615            pulse: pulse_out,
616        })
617    }
618
619    pub fn first(&self) -> Point {
620        self.calibration.first()
621    }
622
623    pub fn mid_value(&self) -> f32 {
624        (self.calibration.first().value + self.calibration.last().value) / 2.0
625    }
626
627    pub fn last(&self) -> Point {
628        self.calibration.last()
629    }
630}
631
632pub fn map_float(value: f32, in_min: f32, in_max: f32, out_min: f32, out_max: f32) -> f32 {
633    ((value - in_min) * (out_max - out_min)) / (in_max - in_min) + out_min
634}
servo_pio/calibration.rs

servo_pio/
calibration.rs
