cu_rp_encoder/
lib.rs

1#[cfg(mock)]
2mod mock;
3
4use bincode::{Decode, Encode};
5use cu29::CuResult;
6use cu29::prelude::*;
7use std::sync::{Arc, Mutex};
8
9#[allow(unused_imports)]
10use cu29_traits::CuError;
11
12#[cfg(mock)]
13use mock::{InputPin, get_pin};
14#[cfg(hardware)]
15use rppal::gpio::{Gpio, InputPin, Level, Trigger};
16use serde::Deserialize;
17
18#[allow(dead_code)]
19struct InterruptData {
20    dat_pin: InputPin,
21    ticks: i32,
22    tov: CuDuration,
23}
24
25#[cfg(hardware)]
26fn get_pin(pin_nb: u8) -> CuResult<InputPin> {
27    // Gpio manages a singleton behind the scene.
28    Ok(Gpio::new()
29        .expect("Could not create GPIO bindings")
30        .get(pin_nb)
31        .map_err(|e| CuError::new_with_cause("Could not get pin", e))?
32        .into_input())
33}
34
35#[derive(Default, Clone, Debug, Encode, Decode, Serialize, Deserialize)]
36pub struct EncoderPayload {
37    pub ticks: i32,
38}
39
40/// That allows the interfacing with the GenericPID
41impl From<&EncoderPayload> for f32 {
42    fn from(payload: &EncoderPayload) -> f32 {
43        payload.ticks as f32
44    }
45}
46
47#[allow(dead_code)]
48pub struct Encoder {
49    clk_pin: InputPin,
50    data_from_interrupts: Arc<Mutex<InterruptData>>,
51}
52
53impl Freezable for Encoder {
54    // pin is derived from the config, so we keep the default implementation.
55}
56
57impl CuSrcTask for Encoder {
58    type Resources<'r> = ();
59    type Output<'m> = output_msg!(EncoderPayload);
60
61    fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
62    where
63        Self: Sized,
64    {
65        let ComponentConfig(config) =
66            config.ok_or("Encoder needs a config with clk_pin and dat_pin.")?;
67
68        let clk_pin_nb_value = config.get("clk_pin").ok_or("Encoder needs a clk_pin")?;
69        let clk_pin: u8 = clk_pin_nb_value.clone().into();
70
71        let dat_pin_nb_value = config.get("dat_pin").ok_or("Encoder needs a dat_pin")?;
72        let dat_pin: u8 = dat_pin_nb_value.clone().into();
73
74        let clk_pin: InputPin = get_pin(clk_pin)?;
75        let dat_pin: InputPin = get_pin(dat_pin)?;
76
77        Ok(Self {
78            clk_pin,
79            data_from_interrupts: Arc::new(Mutex::new(InterruptData {
80                dat_pin,
81                ticks: 0,
82                tov: CuDuration::default(),
83            })),
84        })
85    }
86
87    #[allow(unused_variables)]
88    fn start(&mut self, clock: &RobotClock) -> CuResult<()> {
89        let clock = clock.clone();
90        let idata = Arc::clone(&self.data_from_interrupts);
91        #[cfg(hardware)]
92        self.clk_pin
93            .set_async_interrupt(Trigger::FallingEdge, None, move |_| {
94                let mut idata = idata.lock().unwrap();
95                if idata.dat_pin.read() == Level::Low {
96                    idata.ticks -= 1;
97                } else {
98                    idata.ticks += 1;
99                }
100                idata.tov = clock.now();
101            })
102            .map_err(|e| CuError::new_with_cause("Failed to set async interrupt", e))?;
103        Ok(())
104    }
105
106    fn process(&mut self, clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
107        let idata = self.data_from_interrupts.lock().unwrap();
108        new_msg.tov = Some(clock.now()).into();
109        new_msg.metadata.set_status(idata.ticks);
110        new_msg.set_payload(EncoderPayload { ticks: idata.ticks });
111        Ok(())
112    }
113    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
114        #[cfg(hardware)]
115        self.clk_pin
116            .clear_async_interrupt()
117            .map_err(|e| CuError::new_with_cause("Failed to reset async interrupt", e))?;
118        Ok(())
119    }
120}
cu_rp_encoder/lib.rs

cu_rp_encoder/
lib.rs
