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

use core::fmt::Debug;
use core::task::Poll;
use defmt::Format;
use embedded_hal::digital::v2::{OutputPin, PinState};
use fugit::TimerDurationU32 as TimerDuration;
use fugit_timer::Timer;
use nb;

use super::Actuator;

#[derive(Clone, Copy, Debug, Format)]
pub enum LedAction<const TIMER_HZ: u32> {
    Set { is_on: bool },
    Blink { duration: TimerDuration<TIMER_HZ> },
}

#[derive(Clone, Copy, Debug, Format)]
enum LedBlinkStatus {
    Start,
    Wait,
    Done,
}

#[derive(Clone, Copy, Debug, Format)]
enum LedState<const TIMER_HZ: u32> {
    Set {
        is_on: bool,
    },
    Blink {
        status: LedBlinkStatus,
        duration: TimerDuration<TIMER_HZ>,
    },
}

#[derive(Clone, Copy, Debug, Format)]
pub struct LedDevice<P, T, const TIMER_HZ: u32>
where
    P: OutputPin,
    T: Timer<TIMER_HZ>,
{
    pin: P,
    timer: T,
    state: Option<LedState<TIMER_HZ>>,
}

impl<P, T, const TIMER_HZ: u32> LedDevice<P, T, TIMER_HZ>
where
    P: OutputPin,
    T: Timer<TIMER_HZ>,
{
    pub fn new(pin: P, timer: T) -> Self {
        Self {
            pin,
            timer,
            state: None,
        }
    }
}

#[derive(Clone, Copy, Debug)]
pub enum LedError<PinError: Debug, TimerError: Debug> {
    PinSet(PinError),
    TimerStart(TimerError),
    TimerWait(TimerError),
}

impl<P, T, const TIMER_HZ: u32> Actuator for LedDevice<P, T, TIMER_HZ>
where
    P: OutputPin,
    P::Error: Debug,
    T: Timer<TIMER_HZ>,
    T::Error: Debug,
{
    type Action = LedAction<TIMER_HZ>;
    type Error = LedError<P::Error, T::Error>;

    fn run(&mut self, action: &Self::Action) {
        match action {
            LedAction::Set { is_on } => {
                self.state = Some(LedState::Set { is_on: *is_on });
            }
            LedAction::Blink { duration } => {
                self.state = Some(LedState::Blink {
                    status: LedBlinkStatus::Start,
                    duration: *duration,
                });
            }
        }
    }

    fn poll(&mut self) -> Poll<Result<(), Self::Error>> {
        match self.state {
            Some(LedState::Set { is_on }) => {
                // set led state
                self.pin
                    .set_state(PinState::from(is_on))
                    .map_err(LedError::PinSet)?;

                self.state = None;

                Poll::Ready(Ok(()))
            }
            Some(LedState::Blink { duration, status }) => {
                match status {
                    LedBlinkStatus::Start => {
                        // start timer
                        self.timer.start(duration).map_err(LedError::TimerStart)?;

                        // turn led on
                        self.pin.set_high().map_err(LedError::PinSet)?;

                        // update state
                        self.state = Some(LedState::Blink {
                            status: LedBlinkStatus::Wait,
                            duration,
                        });

                        Poll::Pending
                    }
                    LedBlinkStatus::Wait => match self.timer.wait() {
                        Err(nb::Error::Other(err)) => Poll::Ready(Err(LedError::TimerWait(err))),
                        Err(nb::Error::WouldBlock) => Poll::Pending,
                        Ok(()) => {
                            self.state = Some(LedState::Blink {
                                status: LedBlinkStatus::Done,
                                duration,
                            });

                            Poll::Pending
                        }
                    },
                    LedBlinkStatus::Done => {
                        self.pin.set_low().map_err(LedError::PinSet)?;

                        self.state = None;

                        Poll::Ready(Ok(()))
                    }
                }
            }
            None => Poll::Ready(Ok(())),
        }
    }
}