cu_rp_encoder/
lib.rs

1#[cfg(mock)]
2mod mock;
3
4use bincode::{Decode, Encode};
5use cu29::prelude::*;
6use cu29::CuResult;
7use std::sync::{Arc, Mutex};
8
9#[allow(unused_imports)]
10use cu29_traits::CuError;
11
12#[cfg(mock)]
13use mock::{get_pin, InputPin};
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 Output<'m> = output_msg!(EncoderPayload);
59
60    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
61    where
62        Self: Sized,
63    {
64        let ComponentConfig(config) =
65            config.ok_or("Encoder needs a config with clk_pin and dat_pin.")?;
66
67        let clk_pin_nb_value = config.get("clk_pin").ok_or("Encoder needs a clk_pin")?;
68        let clk_pin: u8 = clk_pin_nb_value.clone().into();
69
70        let dat_pin_nb_value = config.get("dat_pin").ok_or("Encoder needs a dat_pin")?;
71        let dat_pin: u8 = dat_pin_nb_value.clone().into();
72
73        let clk_pin: InputPin = get_pin(clk_pin)?;
74        let dat_pin: InputPin = get_pin(dat_pin)?;
75
76        Ok(Self {
77            clk_pin,
78            data_from_interrupts: Arc::new(Mutex::new(InterruptData {
79                dat_pin,
80                ticks: 0,
81                tov: CuDuration::default(),
82            })),
83        })
84    }
85
86    #[allow(unused_variables)]
87    fn start(&mut self, clock: &RobotClock) -> CuResult<()> {
88        let clock = clock.clone();
89        let idata = Arc::clone(&self.data_from_interrupts);
90        #[cfg(hardware)]
91        self.clk_pin
92            .set_async_interrupt(Trigger::FallingEdge, None, move |_| {
93                let mut idata = idata.lock().unwrap();
94                if idata.dat_pin.read() == Level::Low {
95                    idata.ticks -= 1;
96                } else {
97                    idata.ticks += 1;
98                }
99                idata.tov = clock.now();
100            })
101            .map_err(|e| CuError::new_with_cause("Failed to set async interrupt", e))?;
102        Ok(())
103    }
104
105    fn process(&mut self, clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
106        let idata = self.data_from_interrupts.lock().unwrap();
107        new_msg.tov = Some(clock.now()).into();
108        new_msg.metadata.set_status(idata.ticks);
109        new_msg.set_payload(EncoderPayload { ticks: idata.ticks });
110        Ok(())
111    }
112    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
113        #[cfg(hardware)]
114        self.clk_pin
115            .clear_async_interrupt()
116            .map_err(|e| CuError::new_with_cause("Failed to reset async interrupt", e))?;
117        Ok(())
118    }
119}
cu_rp_encoder/lib.rs

cu_rp_encoder/
lib.rs
