cu_rp_encoder/
lib.rs

1#[cfg(mock)]
2mod mock;
3
4use bincode::{Decode, Encode};
5use cu29::CuResult;
6use cu29::prelude::*;
7#[cfg(hardware)]
8use cu29::resource::Owned;
9use cu29::resource::{ResourceBindingMap, ResourceBindings, ResourceManager};
10use std::sync::{Arc, Mutex};
11
12#[allow(unused_imports)]
13use cu29_traits::CuError;
14
15#[cfg(hardware)]
16use cu_linux_resources::LinuxInputPin;
17#[cfg(mock)]
18use mock::{InputPin, get_pin};
19#[cfg(hardware)]
20use rppal::gpio::{Level, Trigger};
21use serde::Deserialize;
22
23#[cfg(hardware)]
24type InputPin = LinuxInputPin;
25
26#[derive(Copy, Clone, Debug, Eq, PartialEq)]
27pub enum Binding {
28    ClkPin,
29    DatPin,
30}
31
32pub struct EncoderResources {
33    #[cfg(hardware)]
34    pub clk_pin: Owned<InputPin>,
35    #[cfg(hardware)]
36    pub dat_pin: Owned<InputPin>,
37}
38
39impl<'r> ResourceBindings<'r> for EncoderResources {
40    type Binding = Binding;
41
42    fn from_bindings(
43        manager: &'r mut ResourceManager,
44        mapping: Option<&ResourceBindingMap<Self::Binding>>,
45    ) -> CuResult<Self> {
46        #[cfg(hardware)]
47        {
48            let mapping = mapping.ok_or_else(|| {
49                CuError::from("Encoder requires `clk_pin` and `dat_pin` resource mappings")
50            })?;
51            let clk_pin = mapping.get(Binding::ClkPin).ok_or_else(|| {
52                CuError::from("Encoder resources must include `clk_pin: <bundle.resource>`")
53            })?;
54            let dat_pin = mapping.get(Binding::DatPin).ok_or_else(|| {
55                CuError::from("Encoder resources must include `dat_pin: <bundle.resource>`")
56            })?;
57            let clk_pin = manager
58                .take::<InputPin>(clk_pin.typed())
59                .map_err(|e| e.add_cause("Failed to fetch encoder clk pin resource"))?;
60            let dat_pin = manager
61                .take::<InputPin>(dat_pin.typed())
62                .map_err(|e| e.add_cause("Failed to fetch encoder dat pin resource"))?;
63            Ok(Self { clk_pin, dat_pin })
64        }
65        #[cfg(mock)]
66        {
67            let _ = manager;
68            let _ = mapping;
69            Ok(Self {})
70        }
71    }
72}
73
74#[allow(dead_code)]
75struct InterruptData {
76    dat_pin: InputPin,
77    ticks: i32,
78    tov: CuDuration,
79}
80
81#[derive(Default, Clone, Debug, Encode, Decode, Serialize, Deserialize, Reflect)]
82pub struct EncoderPayload {
83    pub ticks: i32,
84}
85
86/// That allows the interfacing with the GenericPID
87impl From<&EncoderPayload> for f32 {
88    fn from(payload: &EncoderPayload) -> f32 {
89        payload.ticks as f32
90    }
91}
92
93#[allow(dead_code)]
94#[derive(Reflect)]
95#[reflect(from_reflect = false)]
96pub struct Encoder {
97    #[reflect(ignore)]
98    clk_pin: InputPin,
99    #[reflect(ignore)]
100    data_from_interrupts: Arc<Mutex<InterruptData>>,
101}
102
103impl Freezable for Encoder {
104    // pin is derived from the config/resources, so we keep the default implementation.
105}
106
107impl CuSrcTask for Encoder {
108    type Resources<'r> = EncoderResources;
109    type Output<'m> = output_msg!(EncoderPayload);
110
111    fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
112    where
113        Self: Sized,
114    {
115        #[cfg(hardware)]
116        let clk_pin: InputPin = _resources.clk_pin.0;
117
118        #[cfg(mock)]
119        let clk_pin: InputPin = {
120            let clk_pin = pin_from_config(_config, "clk_pin")?;
121            get_pin(clk_pin)?
122        };
123
124        #[cfg(hardware)]
125        let dat_pin: InputPin = _resources.dat_pin.0;
126
127        #[cfg(mock)]
128        let dat_pin: InputPin = {
129            let dat_pin = pin_from_config(_config, "dat_pin")?;
130            get_pin(dat_pin)?
131        };
132
133        Ok(Self {
134            clk_pin,
135            data_from_interrupts: Arc::new(Mutex::new(InterruptData {
136                dat_pin,
137                ticks: 0,
138                tov: CuDuration::default(),
139            })),
140        })
141    }
142
143    #[allow(unused_variables)]
144    fn start(&mut self, clock: &RobotClock) -> CuResult<()> {
145        let clock = clock.clone();
146        let idata = Arc::clone(&self.data_from_interrupts);
147        #[cfg(hardware)]
148        self.clk_pin
149            .get_mut()
150            .set_async_interrupt(Trigger::FallingEdge, None, move |_| {
151                let mut idata = idata.lock().unwrap();
152                if idata.dat_pin.get_mut().read() == Level::Low {
153                    idata.ticks -= 1;
154                } else {
155                    idata.ticks += 1;
156                }
157                idata.tov = clock.now();
158            })
159            .map_err(|e| CuError::new_with_cause("Failed to set async interrupt", e))?;
160        Ok(())
161    }
162
163    fn process(&mut self, clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
164        let idata = self.data_from_interrupts.lock().unwrap();
165        new_msg.tov = Some(clock.now()).into();
166        new_msg.metadata.set_status(idata.ticks);
167        new_msg.set_payload(EncoderPayload { ticks: idata.ticks });
168        Ok(())
169    }
170    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
171        #[cfg(hardware)]
172        self.clk_pin
173            .get_mut()
174            .clear_async_interrupt()
175            .map_err(|e| CuError::new_with_cause("Failed to reset async interrupt", e))?;
176        Ok(())
177    }
178}
179
180#[cfg(mock)]
181fn pin_from_config(config: Option<&ComponentConfig>, key: &str) -> CuResult<u8> {
182    let config = config.ok_or("Encoder needs a config with clk_pin and dat_pin.")?;
183    config
184        .get::<u8>(key)?
185        .ok_or_else(|| CuError::from(format!("Encoder needs a {key}")))
186}
cu_rp_encoder/lib.rs

cu_rp_encoder/
lib.rs
