robokit 0.3.0

Build custom firmware for simple robotic automation
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162

// inspired by https://github.com/rubberduck203/switch-hal

use core::fmt::Debug;
use core::marker::PhantomData;
use defmt::Format;
use embedded_hal::digital::v2::InputPin;
use fugit::{MillisDurationU32 as MillisDuration, TimerDurationU32 as TimerDuration};
use fugit_timer::Timer;
use nb;

use super::Sensor;

#[derive(Copy, Clone, Debug, Format, PartialEq)]
pub enum SwitchStatus {
    On,
    Off,
}

#[derive(Copy, Clone, Debug, Format)]
pub struct SwitchUpdate {
    pub status: SwitchStatus,
}

pub struct SwitchActiveLow;
pub struct SwitchActiveHigh;

#[derive(Copy, Clone, Debug, Format)]
pub struct SwitchDevice<Pin, ActiveLevel, Tim, const TIMER_HZ: u32>
where
    Pin: InputPin,
    Tim: Timer<TIMER_HZ>,
{
    pin: Pin,
    timer: Tim,
    current_status: Option<SwitchStatus>,
    active_level: PhantomData<ActiveLevel>,
    is_debouncing: bool,
}

impl<Pin, ActiveLevel, Tim, const TIMER_HZ: u32> SwitchDevice<Pin, ActiveLevel, Tim, TIMER_HZ>
where
    Pin: InputPin,
    Tim: Timer<TIMER_HZ>,
{
    pub fn new(pin: Pin, timer: Tim) -> Self {
        Self {
            pin,
            timer,
            current_status: None,
            active_level: PhantomData::<ActiveLevel>,
            is_debouncing: false,
        }
    }
}

impl<Pin, Tim, const TIMER_HZ: u32> SwitchDevice<Pin, SwitchActiveHigh, Tim, TIMER_HZ>
where
    Pin: InputPin,
    Tim: Timer<TIMER_HZ>,
{
    pub fn new_active_high(pin: Pin, timer: Tim) -> Self {
        SwitchDevice::<Pin, SwitchActiveHigh, Tim, TIMER_HZ>::new(pin, timer)
    }
}

impl<Pin, Tim, const TIMER_HZ: u32> SwitchDevice<Pin, SwitchActiveLow, Tim, TIMER_HZ>
where
    Pin: InputPin,
    Tim: Timer<TIMER_HZ>,
{
    pub fn new_active_low(pin: Pin, timer: Tim) -> Self {
        SwitchDevice::<Pin, SwitchActiveLow, Tim, TIMER_HZ>::new(pin, timer)
    }
}

pub trait InputSwitch {
    type Error: Debug;

    fn is_active(&self) -> Result<bool, Self::Error>;
}

impl<Pin, Tim, const TIMER_HZ: u32> InputSwitch
    for SwitchDevice<Pin, SwitchActiveLow, Tim, TIMER_HZ>
where
    Pin: InputPin,
    Pin::Error: Debug,
    Tim: Timer<TIMER_HZ>,
{
    type Error = <Pin as InputPin>::Error;

    fn is_active(&self) -> Result<bool, Self::Error> {
        self.pin.is_low()
    }
}

impl<Pin, Tim, const TIMER_HZ: u32> InputSwitch
    for SwitchDevice<Pin, SwitchActiveHigh, Tim, TIMER_HZ>
where
    Pin: InputPin,
    Pin::Error: Debug,
    Tim: Timer<TIMER_HZ>,
{
    type Error = <Pin as InputPin>::Error;

    fn is_active(&self) -> Result<bool, Self::Error> {
        self.pin.is_high()
    }
}

#[derive(Clone, Copy, Debug, Format)]
pub enum SwitchError<PinError: Debug, TimerError: Debug> {
    PinRead(PinError),
    TimerStart(TimerError),
    TimerWait(TimerError),
    TimerCancel(TimerError),
}

impl<Pin, ActiveLevel, Tim, const TIMER_HZ: u32> Sensor
    for SwitchDevice<Pin, ActiveLevel, Tim, TIMER_HZ>
where
    Self: InputSwitch,
    Pin: InputPin,
    Tim: Timer<TIMER_HZ>,
{
    type Message = SwitchUpdate;
    type Error = SwitchError<<Self as InputSwitch>::Error, Tim::Error>;

    fn sense(&mut self) -> Result<Option<SwitchUpdate>, Self::Error> {
        if self.is_debouncing {
            match self.timer.wait() {
                Ok(()) => {
                    self.timer.cancel().map_err(SwitchError::TimerCancel)?;
                    self.is_debouncing = false;
                }
                Err(nb::Error::WouldBlock) => return Ok(None),
                Err(nb::Error::Other(err)) => return Err(SwitchError::TimerWait(err)),
            }
        }

        let is_active = self.is_active().map_err(SwitchError::PinRead)?;

        let status = match is_active {
            true => SwitchStatus::On,
            false => SwitchStatus::Off,
        };

        if Some(status) != self.current_status {
            self.current_status = Some(status);

            let debounce_duration: TimerDuration<TIMER_HZ> =
                MillisDuration::from_ticks(2).convert();
            self.is_debouncing = true;
            self.timer
                .start(debounce_duration)
                .map_err(SwitchError::TimerStart)?;

            Ok(Some(SwitchUpdate { status }))
        } else {
            Ok(None)
        }
    }
}