cu_rp_encoder/
lib.rs

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

cu_rp_encoder/
lib.rs
