rktk_drivers_common/
encoder.rs

1//! Encoder driver implementations.
2
3use embassy_futures::select::{select, select_slice};
4use embedded_hal::digital::InputPin;
5use embedded_hal_async::digital::Wait;
6use rktk::drivers::interface::encoder::{EncoderDirection, EncoderDriver};
7
8const TRANSITIONS: [i8; 16] = [0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0];
9
10struct SingleEncoderState<PIN> {
11    a: PIN,
12    b: PIN,
13    last_state: u8,
14    accumulator: i8,
15}
16
17/// General encoder driver that can be used with any digital input pin.
18pub struct GeneralEncoder<PIN: Wait + InputPin, const ENCODER_COUNT: usize> {
19    encoders: [SingleEncoderState<PIN>; ENCODER_COUNT],
20    resolution: i8,
21}
22
23impl<PIN: Wait + InputPin, const ENCODER_COUNT: usize> GeneralEncoder<PIN, ENCODER_COUNT> {
24    pub fn new(encoders: [(PIN, PIN); ENCODER_COUNT]) -> Self {
25        Self::new_with_resolution(encoders, 4)
26    }
27
28    pub fn new_with_resolution(encoders: [(PIN, PIN); ENCODER_COUNT], resolution: i8) -> Self {
29        let encoders = encoders.map(|(mut a, mut b)| {
30            let a_high = a.is_high().unwrap_or(false);
31            let b_high = b.is_high().unwrap_or(false);
32            let last_state = ((a_high as u8) << 1) | (b_high as u8);
33            SingleEncoderState { a, b, last_state, accumulator: 0 }
34        });
35
36        Self { encoders, resolution: if resolution <= 0 { 1 } else { resolution } }
37    }
38}
39
40impl<PIN: Wait + InputPin, const ENCODER_COUNT: usize> EncoderDriver
41    for GeneralEncoder<PIN, ENCODER_COUNT>
42{
43    async fn read_wait(&mut self) -> (u8, EncoderDirection) {
44        let resolution = self.resolution;
45        let mut id = 0;
46        let futures = self.encoders.each_mut().map(|enc| {
47            let enc_id = id;
48            id += 1;
49            async move {
50                loop {
51                    let _ = select(enc.a.wait_for_any_edge(), enc.b.wait_for_any_edge()).await;
52
53                    let a_high = enc.a.is_high().unwrap_or(false);
54                    let b_high = enc.b.is_high().unwrap_or(false);
55                    let curr_state = ((a_high as u8) << 1) | (b_high as u8);
56
57                    let idx = ((enc.last_state << 2) | curr_state) as usize;
58                    let delta = TRANSITIONS[idx];
59                    enc.last_state = curr_state;
60
61                    if delta != 0 {
62                        enc.accumulator += delta;
63                        if enc.accumulator >= resolution {
64                            enc.accumulator -= resolution;
65                            return (enc_id as u8, EncoderDirection::Clockwise);
66                        } else if enc.accumulator <= -resolution {
67                            enc.accumulator += resolution;
68                            return (enc_id as u8, EncoderDirection::CounterClockwise);
69                        }
70                    }
71                }
72            }
73        });
74
75        select_slice(core::pin::pin!(futures)).await.0
76    }
77}
78
79#[cfg(test)]
80mod tests {
81    use super::*;
82
83    #[test]
84    fn test_quadrature_clockwise_sequence() {
85        // Standard CW sequence starting at rest (1,1): 11 -> 01 -> 00 -> 10 -> 11
86        let cw_states = [0b11, 0b01, 0b00, 0b10, 0b11];
87        let mut acc = 0;
88        let mut last_state = cw_states[0];
89
90        for &curr_state in &cw_states[1..] {
91            let idx = ((last_state << 2) | curr_state) as usize;
92            let delta = TRANSITIONS[idx];
93            assert_eq!(delta, 1, "CW transition should add +1");
94            acc += delta;
95            last_state = curr_state;
96        }
97
98        assert_eq!(acc, 4, "Full CW turn should accumulate +4");
99    }
100
101    #[test]
102    fn test_quadrature_counter_clockwise_sequence() {
103        // Standard CCW sequence starting at rest (1,1): 11 -> 10 -> 00 -> 01 -> 11
104        let ccw_states = [0b11, 0b10, 0b00, 0b01, 0b11];
105        let mut acc = 0;
106        let mut last_state = ccw_states[0];
107
108        for &curr_state in &ccw_states[1..] {
109            let idx = ((last_state << 2) | curr_state) as usize;
110            let delta = TRANSITIONS[idx];
111            assert_eq!(delta, -1, "CCW transition should subtract -1");
112            acc += delta;
113            last_state = curr_state;
114        }
115
116        assert_eq!(acc, -4, "Full CCW turn should accumulate -4");
117    }
118
119    #[test]
120    fn test_quadrature_invalid_double_flip() {
121        let idx = ((0b00 << 2) | 0b11) as usize;
122        assert_eq!(TRANSITIONS[idx], 0, "Double-bit flip should yield 0 delta");
123    }
124}
rktk_drivers_common/encoder.rs

rktk_drivers_common/
encoder.rs
