rrtk/
devices.rs

1// SPDX-License-Identifier: BSD-3-Clause
2// Copyright 2024 UxuginPython
3//!RRTK's device system works through a graph-like structure where each device holds objects called
4//!terminals in [`RefCell`]s. Terminals represent anywhere that a device can connect to another.
5//!Connected terminals hold references to eachother's [`RefCell`]s. This module holds builtin
6//!devices.
7use crate::*;
8pub mod wrappers;
9///A device such that positive for one terminal is negative for the other.
10///As this device has only one degree of freedom, it propagates [`Command`]s given to its terminals
11///as well as [`State`]s.
12pub struct Invert<'a, E: Copy + Debug> {
13    term1: RefCell<Terminal<'a, E>>,
14    term2: RefCell<Terminal<'a, E>>,
15}
16impl<'a, E: Copy + Debug> Invert<'a, E> {
17    ///Constructor for [`Invert`].
18    pub const fn new() -> Self {
19        Self {
20            term1: Terminal::new(),
21            term2: Terminal::new(),
22        }
23    }
24    ///Get a reference to the side 1 terminal of the invert device.
25    pub fn get_terminal_1(&self) -> &'a RefCell<Terminal<'a, E>> {
26        //We don't want to extend the `&self` reference beyond the scope of the function, but we
27        //need need the `term` reference to last for 'a, so we do this to get a reference with a
28        //longer lifetime. This should be OK since both terminals are restricted to the 'a
29        //lifetime.
30        unsafe { &*(&self.term1 as *const RefCell<Terminal<'a, E>>) }
31    }
32    ///Get a reference to the side 2 terminal of the invert device.
33    pub fn get_terminal_2(&self) -> &'a RefCell<Terminal<'a, E>> {
34        unsafe { &*(&self.term2 as *const RefCell<Terminal<'a, E>>) }
35    }
36}
37impl<E: Copy + Debug> Updatable<E> for Invert<'_, E> {
38    fn update(&mut self) -> NothingOrError<E> {
39        self.update_terminals()?;
40        let get1: Option<Datum<State>> = self
41            .term1
42            .borrow()
43            .get()
44            .expect("Terminal get will always return Ok");
45        let get2: Option<Datum<State>> = self
46            .term2
47            .borrow()
48            .get()
49            .expect("Terminal get will always return Ok");
50        match get1 {
51            None => match get2 {
52                None => {}
53                Some(datum2) => {
54                    let newdatum1 = Datum::new(datum2.time, -datum2.value);
55                    self.term1.borrow_mut().set(newdatum1)?;
56                }
57            },
58            Some(datum1) => match get2 {
59                None => {
60                    let newdatum2 = Datum::new(datum1.time, -datum1.value);
61                    self.term2.borrow_mut().set(newdatum2)?;
62                }
63                Some(datum2) => {
64                    let state1 = datum1.value;
65                    let state2 = datum2.value;
66                    let time = if datum1.time >= datum2.time {
67                        datum1.time
68                    } else {
69                        datum2.time
70                    };
71                    //average with negative state2 as it is inverted from state1
72                    let new_state = (state1 - state2) / 2.0;
73                    self.term1.borrow_mut().set(Datum::new(time, new_state))?;
74                    self.term2.borrow_mut().set(Datum::new(time, -new_state))?;
75                }
76            },
77        }
78        let get1: Option<Datum<Command>> = self
79            .term1
80            .borrow()
81            .get()
82            .expect("Terminal get will always return Ok");
83        let get2: Option<Datum<Command>> = self
84            .term2
85            .borrow()
86            .get()
87            .expect("Terminal get will always return Ok");
88        let mut maybe_datum: Option<Datum<Command>> = None;
89        maybe_datum.replace_if_none_or_older_than_option(get1);
90        match get2 {
91            Some(x) => {
92                maybe_datum.replace_if_none_or_older_than(-x);
93            }
94            None => {}
95        }
96        match maybe_datum {
97            Some(datum_command) => {
98                self.term1.borrow_mut().set(datum_command)?;
99                self.term2.borrow_mut().set(-datum_command)?;
100            }
101            None => {}
102        }
103        Ok(())
104    }
105}
106impl<E: Copy + Debug> Device<E> for Invert<'_, E> {
107    fn update_terminals(&mut self) -> NothingOrError<E> {
108        self.term1.borrow_mut().update()?;
109        self.term2.borrow_mut().update()?;
110        Ok(())
111    }
112}
113///A gear train, a mechanism consisting of a two or more gears meshed together.
114///As this device has only one degree of freedom, it propagates [`Command`]s given to its terminals
115///as well as [`State`]s.
116pub struct GearTrain<'a, E: Copy + Debug> {
117    term1: RefCell<Terminal<'a, E>>,
118    term2: RefCell<Terminal<'a, E>>,
119    ratio: f32,
120}
121impl<'a, E: Copy + Debug> GearTrain<'a, E> {
122    ///Construct a [`GearTrain`] with the ratio as an `f32`.
123    pub const fn with_ratio_raw(ratio: f32) -> Self {
124        Self {
125            term1: Terminal::new(),
126            term2: Terminal::new(),
127            ratio: ratio,
128        }
129    }
130    ///Construct a [`GearTrain`] with the ratio as a dimensionless [`Quantity`].
131    pub const fn with_ratio(ratio: Quantity) -> Self {
132        ratio.unit.assert_eq_assume_ok(&DIMENSIONLESS);
133        Self::with_ratio_raw(ratio.value)
134    }
135    ///Construct a [`GearTrain`] from an array of the numbers of teeth on each gear in the train.
136    pub const fn new<const N: usize>(teeth: [f32; N]) -> Self {
137        if N < 2 {
138            panic!("rrtk::devices::GearTrain::new must be provided with at least two gear tooth counts.");
139        }
140        let ratio = teeth[0] / teeth[teeth.len() - 1] * if N % 2 == 0 { -1.0 } else { 1.0 };
141        Self::with_ratio_raw(ratio)
142    }
143    ///Get a reference to the side 1 terminal of the device where (side 1) * ratio = (side 2).
144    pub fn get_terminal_1(&self) -> &'a RefCell<Terminal<'a, E>> {
145        unsafe { &*(&self.term1 as *const RefCell<Terminal<'a, E>>) }
146    }
147    ///Get a reference to the side 2 terminal of the device where (side 1) * ratio = (side 2).
148    pub fn get_terminal_2(&self) -> &'a RefCell<Terminal<'a, E>> {
149        unsafe { &*(&self.term2 as *const RefCell<Terminal<'a, E>>) }
150    }
151}
152impl<E: Copy + Debug> Updatable<E> for GearTrain<'_, E> {
153    fn update(&mut self) -> NothingOrError<E> {
154        self.update_terminals()?;
155        let get1: Option<Datum<State>> = self
156            .term1
157            .borrow()
158            .get()
159            .expect("Terminal get will always return Ok");
160        let get2: Option<Datum<State>> = self
161            .term2
162            .borrow()
163            .get()
164            .expect("Terminal get will always return Ok");
165        match get1 {
166            Some(datum1) => match get2 {
167                Some(datum2) => {
168                    let state1 = datum1.value;
169                    let state2 = datum2.value;
170                    let time = if datum1.time >= datum2.time {
171                        datum1.time
172                    } else {
173                        datum2.time
174                    };
175                    //https://www.desmos.com/3d/gvwbqszr5e
176                    let r_squared_plus_1 = self.ratio * self.ratio + 1.0;
177                    let x_plus_r_y = state1 + state2 * self.ratio;
178                    let newstate1 = x_plus_r_y / r_squared_plus_1;
179                    let newstate2 = (x_plus_r_y * self.ratio) / r_squared_plus_1;
180                    self.term1.borrow_mut().set(Datum::new(time, newstate1))?;
181                    self.term2.borrow_mut().set(Datum::new(time, newstate2))?;
182                }
183                None => {
184                    let newdatum2 = datum1 * self.ratio;
185                    self.term2.borrow_mut().set(newdatum2)?;
186                }
187            },
188            None => match get2 {
189                Some(datum2) => {
190                    let newdatum1 = datum2 / self.ratio;
191                    self.term1.borrow_mut().set(newdatum1)?;
192                }
193                None => {}
194            },
195        }
196        let get1: Option<Datum<Command>> = self
197            .term1
198            .borrow()
199            .get()
200            .expect("Terminal get will always return Ok");
201        let get2: Option<Datum<Command>> = self
202            .term2
203            .borrow()
204            .get()
205            .expect("Terminal get will always return Ok");
206        match get1 {
207            Some(datum1) => match get2 {
208                Some(datum2) => {
209                    if datum1.time >= datum2.time {
210                        let newdatum2 = datum1 * self.ratio;
211                        self.term2.borrow_mut().set(newdatum2)?;
212                    } else {
213                        let newdatum1 = datum2 / self.ratio;
214                        self.term1.borrow_mut().set(newdatum1)?;
215                    }
216                }
217                None => {
218                    let newdatum2 = datum1 * self.ratio;
219                    self.term2.borrow_mut().set(newdatum2)?;
220                }
221            },
222            None => match get2 {
223                Some(datum2) => {
224                    let newdatum1 = datum2 / self.ratio;
225                    self.term1.borrow_mut().set(newdatum1)?;
226                }
227                None => {}
228            },
229        }
230        Ok(())
231    }
232}
233impl<E: Copy + Debug> Device<E> for GearTrain<'_, E> {
234    fn update_terminals(&mut self) -> NothingOrError<E> {
235        self.term1.borrow_mut().update()?;
236        self.term2.borrow_mut().update()?;
237        Ok(())
238    }
239}
240///A connection between terminals that are not directly connected, such as when three or more
241///terminals are connected. Code-wise, this is almost exactly the same as directly connecting two
242///terminals, but this type can connect more than two terminals. There is some freedom in exactly
243///what you do with each of these ways of connecting terminals and what they represent physically,
244///but the intention is that [`connect`] is for only two and [`Axle`] is for more. Using an [`Axle`] for
245///only two terminals is possible but may have a slight performance cost. (The type even
246///technically allows for only one or even zero connected terminals, but there is almost certainly
247///no legitimate use for this.)
248///As this device has only one degree of freedom, it propagates [`Command`]s given to its terminals
249///as well as [`State`]s.
250pub struct Axle<'a, const N: usize, E: Copy + Debug> {
251    inputs: [RefCell<Terminal<'a, E>>; N],
252}
253impl<'a, const N: usize, E: Copy + Debug> Axle<'a, N, E> {
254    ///Constructor for [`Axle`].
255    pub fn new() -> Self {
256        let mut inputs: [core::mem::MaybeUninit<RefCell<Terminal<'a, E>>>; N] =
257            [const { core::mem::MaybeUninit::uninit() }; N];
258        for i in &mut inputs {
259            i.write(Terminal::new());
260        }
261        //transmute doesn't work well with generics, so this does the same thing through pointers instead.
262        let inputs: [RefCell<Terminal<'a, E>>; N] = unsafe {
263            inputs
264                .as_ptr()
265                .cast::<[RefCell<Terminal<'a, E>>; N]>()
266                .read()
267        };
268        Self { inputs: inputs }
269    }
270    ///Get a reference to one of the axle's terminals.
271    pub fn get_terminal(&self, terminal: usize) -> &'a RefCell<Terminal<'a, E>> {
272        unsafe { &*(&self.inputs[terminal] as *const RefCell<Terminal<'a, E>>) }
273    }
274}
275impl<const N: usize, E: Copy + Debug> Updatable<E> for Axle<'_, N, E> {
276    fn update(&mut self) -> NothingOrError<E> {
277        self.update_terminals()?;
278        let mut count = 0u16;
279        let mut datum = Datum::new(Time(i64::MIN), State::default());
280        for i in &self.inputs {
281            match i.borrow().get()? {
282                Some(gotten_datum) => {
283                    datum += gotten_datum;
284                    count += 1;
285                }
286                None => (),
287            }
288        }
289        if count >= 1 {
290            datum /= count as f32;
291            for i in &self.inputs {
292                i.borrow_mut().set(datum.clone())?;
293            }
294        }
295        let mut maybe_datum: Option<Datum<Command>> = None;
296        for i in &self.inputs {
297            maybe_datum.replace_if_none_or_older_than_option(i.borrow().get()?);
298        }
299        if let Some(datum) = maybe_datum {
300            for i in &self.inputs {
301                i.borrow_mut().set(datum.clone())?;
302            }
303        }
304        Ok(())
305    }
306}
307impl<const N: usize, E: Copy + Debug> Device<E> for Axle<'_, N, E> {
308    fn update_terminals(&mut self) -> NothingOrError<E> {
309        for i in &self.inputs {
310            i.borrow_mut().update()?;
311        }
312        Ok(())
313    }
314}
315///Since each branch of a differential is dependent on the other two, we can calculate each with
316///only the others. This allows you to select a branch to completely calculate and not call
317///[`get`](Terminal::get)
318///on. For example, if you have encoders on two branches, you would probably want to calculate the
319///third from their readings. If you have encoders on all three branches, you can also choose to
320///use all three values from them with the [`Equal`](DifferentialDistrust::Equal) variant.
321pub enum DifferentialDistrust {
322    ///Calculate the state of side 1 from sum and side 2 and do not call [`get`](Terminal::get) on it.
323    Side1,
324    ///Calculate the state of side 2 from sum and side 1 and do not call [`get`](Terminal::get) on it.
325    Side2,
326    ///Calculate the state of sum from side 1 and side 2 and do not call [`get`](Terminal::get) on it.
327    Sum,
328    ///Trust all branches equally in the calculation. Note that this is a bit slower.
329    Equal,
330}
331///A mechanical differential mechanism.
332///As this device has two degrees of freedom, it is not able to propagate [`Command`]s given to its
333///terminals as it does with [`State`]s.
334pub struct Differential<'a, E: Copy + Debug> {
335    side1: RefCell<Terminal<'a, E>>,
336    side2: RefCell<Terminal<'a, E>>,
337    sum: RefCell<Terminal<'a, E>>,
338    distrust: DifferentialDistrust,
339}
340impl<'a, E: Copy + Debug> Differential<'a, E> {
341    ///Constructor for [`Differential`]. Trusts all branches equally.
342    pub const fn new() -> Self {
343        Self {
344            side1: Terminal::new(),
345            side2: Terminal::new(),
346            sum: Terminal::new(),
347            distrust: DifferentialDistrust::Equal,
348        }
349    }
350    ///Constructor for [`Differential`] where you choose what to distrust.
351    pub fn with_distrust(distrust: DifferentialDistrust) -> Self {
352        Self {
353            side1: Terminal::new(),
354            side2: Terminal::new(),
355            sum: Terminal::new(),
356            distrust: distrust,
357        }
358    }
359    ///Get a reference to the side 1 terminal of the differential.
360    pub fn get_side_1(&self) -> &'a RefCell<Terminal<'a, E>> {
361        unsafe { &*(&self.side1 as *const RefCell<Terminal<'a, E>>) }
362    }
363    ///Get a reference to the side 2 terminal of the differential.
364    pub fn get_side_2(&self) -> &'a RefCell<Terminal<'a, E>> {
365        unsafe { &*(&self.side2 as *const RefCell<Terminal<'a, E>>) }
366    }
367    ///Get a reference to the sum terminal of the differential.
368    pub fn get_sum(&self) -> &'a RefCell<Terminal<'a, E>> {
369        unsafe { &*(&self.sum as *const RefCell<Terminal<'a, E>>) }
370    }
371}
372impl<E: Copy + Debug> Updatable<E> for Differential<'_, E> {
373    fn update(&mut self) -> NothingOrError<E> {
374        self.update_terminals()?;
375        match self.distrust {
376            DifferentialDistrust::Side1 => {
377                let sum: Datum<State> = match self.sum.borrow().get()? {
378                    Some(sum) => sum,
379                    None => return Ok(()),
380                };
381                let side2: Datum<State> = match self.side2.borrow().get()? {
382                    Some(side2) => side2,
383                    None => return Ok(()),
384                };
385                self.side1.borrow_mut().set(sum - side2)?;
386            }
387            DifferentialDistrust::Side2 => {
388                let sum: Datum<State> = match self.sum.borrow().get()? {
389                    Some(sum) => sum,
390                    None => return Ok(()),
391                };
392                let side1: Datum<State> = match self.side1.borrow().get()? {
393                    Some(side1) => side1,
394                    None => return Ok(()),
395                };
396                self.side2.borrow_mut().set(sum - side1)?;
397            }
398            DifferentialDistrust::Sum => {
399                let side1: Datum<State> = match self.side1.borrow().get()? {
400                    Some(side1) => side1,
401                    None => return Ok(()),
402                };
403                let side2: Datum<State> = match self.side2.borrow().get()? {
404                    Some(side2) => side2,
405                    None => return Ok(()),
406                };
407                self.sum.borrow_mut().set(side1 + side2)?;
408            }
409            DifferentialDistrust::Equal => {
410                let sum: Datum<State> = match self.sum.borrow().get()? {
411                    Some(sum) => sum,
412                    None => return Ok(()),
413                };
414                let side1: Datum<State> = match self.side1.borrow().get()? {
415                    Some(side1) => side1,
416                    None => return Ok(()),
417                };
418                let side2: Datum<State> = match self.side2.borrow().get()? {
419                    Some(side2) => side2,
420                    None => return Ok(()),
421                };
422                //This minimizes (x-a)^2+(y-b)^2+(z-c)^2 given a+b=c where x, y, and z are the
423                //measured values of side1, side2, and sum respectively and a, b, and c are their
424                //calculated estimated values based on all three constrained to add. This
425                //essentially means that the estimated values will be as close to the measured
426                //values as possible while forcing the two sides to add to the sum branch.
427                self.sum
428                    .borrow_mut()
429                    .set((side1 + side2 + sum * 2.0) / 3.0)?;
430                self.side1
431                    .borrow_mut()
432                    .set((side1 * 2.0 - side2 + sum) / 3.0)?;
433                self.side2
434                    .borrow_mut()
435                    .set((-side1 + side2 * 2.0 + sum) / 3.0)?;
436            }
437        }
438        Ok(())
439    }
440}
441impl<E: Copy + Debug> Device<E> for Differential<'_, E> {
442    fn update_terminals(&mut self) -> NothingOrError<E> {
443        self.side1.borrow_mut().update()?;
444        self.side2.borrow_mut().update()?;
445        self.sum.borrow_mut().update()?;
446        Ok(())
447    }
448}
rrtk/devices.rs

rrtk/
devices.rs
