motor_driver_hal/
wrapper.rs

1use crate::{MotorDriver, MotorDriverError};
2use embedded_hal::digital::OutputPin;
3use embedded_hal::pwm::SetDutyCycle;
4
5pub enum EnablePins<E1, E2> {
6    None,
7    Single(E1),
8    Dual(E1, E2),
9}
10
11pub enum PwmChannels<P1, P2> {
12    None,
13    Single(P1),
14    Dual(P1, P2),
15}
16
17#[derive(Debug, Clone, Copy, PartialEq, Eq)]
18pub enum MotorDirection {
19    Forward,
20    Reverse,
21    Brake,
22    Coast,
23}
24
25pub struct MotorDriverWrapper<E1, E2, P1, P2> {
26    enable_pins: EnablePins<E1, E2>,
27    pwm_channels: PwmChannels<P1, P2>,
28    max_duty: u16,
29    current_speed: i16,
30    current_pulse: i16,
31    ppr: i16,
32    direction: MotorDirection,
33    initialized: bool,
34}
35
36impl<E1, E2, P1, P2> MotorDriverWrapper<E1, E2, P1, P2>
37where
38    E1: OutputPin,
39    E2: OutputPin,
40    P1: SetDutyCycle,
41    P2: SetDutyCycle,
42{
43    pub fn builder() -> MotorDriverBuilder<E1, E2, P1, P2> {
44        MotorDriverBuilder::new()
45    }
46
47    fn control_enable(&mut self, enable: bool) -> Result<(), MotorDriverError> {
48        match &mut self.enable_pins {
49            EnablePins::None => Ok(()),
50            EnablePins::Single(pin) => {
51                if enable {
52                    pin.set_high().map_err(|_| MotorDriverError::GpioError)?;
53                } else {
54                    pin.set_low().map_err(|_| MotorDriverError::GpioError)?;
55                }
56                Ok(())
57            }
58            EnablePins::Dual(pin1, pin2) => {
59                if enable {
60                    pin1.set_high().map_err(|_| MotorDriverError::GpioError)?;
61                    pin2.set_high().map_err(|_| MotorDriverError::GpioError)?;
62                } else {
63                    pin1.set_low().map_err(|_| MotorDriverError::GpioError)?;
64                    pin2.set_low().map_err(|_| MotorDriverError::GpioError)?;
65                }
66                Ok(())
67            }
68        }
69    }
70
71    fn update_pwm(&mut self) -> Result<(), MotorDriverError> {
72        let duty = self.current_speed.unsigned_abs().min(self.max_duty);
73
74        match (&mut self.pwm_channels, self.direction) {
75            (PwmChannels::None, _) => Ok(()),
76            (PwmChannels::Single(pwm), _) => {
77                if self.direction == MotorDirection::Coast {
78                    pwm.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
79                } else {
80                    pwm.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
81                }
82                Ok(())
83            }
84            (PwmChannels::Dual(pwm1, pwm2), MotorDirection::Forward) => {
85                pwm1.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
86                pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
87                Ok(())
88            }
89            (PwmChannels::Dual(pwm1, pwm2), MotorDirection::Reverse) => {
90                pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
91                pwm2.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
92                Ok(())
93            }
94            (PwmChannels::Dual(pwm1, pwm2), MotorDirection::Brake) => {
95                pwm1.set_duty_cycle(self.max_duty).map_err(|_| MotorDriverError::PwmError)?;
96                pwm2.set_duty_cycle(self.max_duty).map_err(|_| MotorDriverError::PwmError)?;
97                Ok(())
98            }
99            (PwmChannels::Dual(pwm1, pwm2), MotorDirection::Coast) => {
100                pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
101                pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
102                Ok(())
103            }
104        }
105    }
106}
107
108impl<E1, E2, P1, P2> MotorDriver for MotorDriverWrapper<E1, E2, P1, P2>
109where
110    E1: OutputPin,
111    E2: OutputPin,
112    P1: SetDutyCycle,
113    P2: SetDutyCycle,
114{
115    type Error = MotorDriverError;
116
117    fn initialize(&mut self) -> Result<(), Self::Error> {
118        self.control_enable(false)?;
119        
120        match &mut self.pwm_channels {
121            PwmChannels::None => {},
122            PwmChannels::Single(pwm) => {
123                pwm.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
124            }
125            PwmChannels::Dual(pwm1, pwm2) => {
126                pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
127                pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
128            }
129        }
130        
131        self.initialized = true;
132        Ok(())
133    }
134
135    fn set_speed(&mut self, speed: i16) -> Result<(), Self::Error> {
136        if !self.initialized {
137            return Err(MotorDriverError::NotInitialized);
138        }
139        
140        if speed.unsigned_abs() > self.max_duty {
141            return Err(MotorDriverError::InvalidSpeed);
142        }
143        
144        self.current_speed = speed;
145        if speed < 0 {
146            self.direction = MotorDirection::Reverse;
147        } else if speed > 0 {
148            self.direction = MotorDirection::Forward;
149        }
150        
151        self.update_pwm()
152    }
153
154    fn set_direction(&mut self, forward: bool) -> Result<(), Self::Error> {
155        if !self.initialized {
156            return Err(MotorDriverError::NotInitialized);
157        }
158        
159        self.direction = if forward {
160            MotorDirection::Forward
161        } else {
162            MotorDirection::Reverse
163        };
164        
165        self.update_pwm()
166    }
167
168    fn stop(&mut self) -> Result<(), Self::Error> {
169        if !self.initialized {
170            return Err(MotorDriverError::NotInitialized);
171        }
172        
173        self.current_speed = 0;
174        self.direction = MotorDirection::Coast;
175        self.update_pwm()
176    }
177
178    fn brake(&mut self) -> Result<(), Self::Error> {
179        if !self.initialized {
180            return Err(MotorDriverError::NotInitialized);
181        }
182        
183        self.current_speed = 0;
184        self.direction = MotorDirection::Brake;
185        self.update_pwm()
186    }
187
188    fn enable(&mut self) -> Result<(), Self::Error> {
189        if !self.initialized {
190            return Err(MotorDriverError::NotInitialized);
191        }
192        
193        self.control_enable(true)
194    }
195
196    fn disable(&mut self) -> Result<(), Self::Error> {
197        if !self.initialized {
198            return Err(MotorDriverError::NotInitialized);
199        }
200        
201        self.control_enable(false)
202    }
203
204    fn get_speed(&self) -> Result<i16, Self::Error> {
205        if !self.initialized {
206            return Err(MotorDriverError::NotInitialized);
207        }
208        Ok(self.current_speed)
209    }
210
211    fn get_direction(&self) -> Result<bool, Self::Error> {
212        if !self.initialized {
213            return Err(MotorDriverError::NotInitialized);
214        }
215        Ok(self.direction == MotorDirection::Forward)
216    }
217
218    fn get_current(&self) -> Result<f32, Self::Error> {
219        Err(MotorDriverError::HardwareFault)
220    }
221
222    fn get_voltage(&self) -> Result<f32, Self::Error> {
223        Err(MotorDriverError::HardwareFault)
224    }
225
226    fn get_temperature(&self) -> Result<f32, Self::Error> {
227        Err(MotorDriverError::HardwareFault)
228    }
229
230    fn get_fault_status(&self) -> Result<u8, Self::Error> {
231        if !self.initialized {
232            return Err(MotorDriverError::NotInitialized);
233        }
234        Ok(0)
235    }
236    
237    fn set_ppr(&mut self, ppr: i16) -> Result<bool, Self::Error> {
238        if !self.initialized {
239            return Err(MotorDriverError::NotInitialized);
240        }
241        self.ppr = ppr;
242        Ok(true)
243    }
244
245    fn check_ppr(&mut self) -> Result<(), Self::Error> {
246        if self.ppr == 0 {
247            return Err(MotorDriverError::NotInitialized);
248        }
249        Ok(())
250    }
251}
252
253pub struct MotorDriverBuilder<E1, E2, P1, P2> {
254    enable_pins: Option<EnablePins<E1, E2>>,
255    pwm_channels: Option<PwmChannels<P1, P2>>,
256    max_duty: Option<u16>,
257    initial_speed: Option<i16>,
258    initial_direction: Option<MotorDirection>,
259    ppr: Option<i16>,
260}
261
262impl<E1, E2, P1, P2> MotorDriverBuilder<E1, E2, P1, P2> {
263    /// Create a new motor driver builder with default settings.
264    /// 
265    /// # Example
266    /// 
267    /// ```rust
268    /// let builder = MotorDriverBuilder::new()
269    ///     .with_single_enable(enable_pin)
270    ///     .with_single_pwm(pwm_channel, 1000);
271    /// ```
272    pub fn new() -> Self {
273        Self {
274            enable_pins: None,
275            pwm_channels: None,
276            max_duty: None,
277            initial_speed: None,
278            initial_direction: None,
279            ppr: None,
280        }
281    }
282
283    pub fn with_single_enable(mut self, enable: E1) -> Self {
284        self.enable_pins = Some(EnablePins::Single(enable));
285        self
286    }
287
288    /// Configure dual enable pins for H-bridge motor control.
289    /// 
290    /// # Arguments
291    /// 
292    /// * `enable1` - First enable pin for one side of H-bridge
293    /// * `enable2` - Second enable pin for other side of H-bridge
294    /// 
295    /// # Example
296    /// 
297    /// ```rust
298    /// builder.with_dual_enable(enable_pin1, enable_pin2)
299    /// ```
300    pub fn with_dual_enable(mut self, enable1: E1, enable2: E2) -> Self {
301        self.enable_pins = Some(EnablePins::Dual(enable1, enable2));
302        self
303    }
304
305    pub fn with_single_pwm(mut self, pwm: P1) -> Self {
306        self.pwm_channels = Some(PwmChannels::Single(pwm));
307        self
308    }
309
310    pub fn with_dual_pwm(mut self, pwm1: P1, pwm2: P2) -> Self {
311        self.pwm_channels = Some(PwmChannels::Dual(pwm1, pwm2));
312        self
313    }
314
315    pub fn with_enable_pins(mut self, pins: EnablePins<E1, E2>) -> Self {
316        self.enable_pins = Some(pins);
317        self
318    }
319
320    pub fn with_pwm_channels(mut self, channels: PwmChannels<P1, P2>) -> Self {
321        self.pwm_channels = Some(channels);
322        self
323    }
324
325    pub fn with_max_duty(mut self, max_duty: u16) -> Self {
326        self.max_duty = Some(max_duty);
327        self
328    }
329
330    pub fn with_initial_speed(mut self, speed: i16) -> Self {
331        self.initial_speed = Some(speed);
332        self
333    }
334
335    pub fn with_initial_direction(mut self, direction: MotorDirection) -> Self {
336        self.initial_direction = Some(direction);
337        self
338    }
339
340    pub fn with_ppr(mut self, ppr: i16) -> Self {
341        self.ppr = Some(ppr);
342        self
343    }
344
345    pub fn build(self) -> MotorDriverWrapper<E1, E2, P1, P2> {
346        MotorDriverWrapper {
347            enable_pins: self.enable_pins.unwrap_or(EnablePins::None),
348            pwm_channels: self.pwm_channels.unwrap_or(PwmChannels::None),
349            max_duty: self.max_duty.unwrap_or(1000),
350            current_speed: self.initial_speed.unwrap_or(0),
351            current_pulse: 0,
352            ppr: self.ppr.unwrap_or(0),
353            direction: self.initial_direction.unwrap_or(MotorDirection::Coast),
354            initialized: false,
355        }
356    }
357
358    pub fn build_and_init(self) -> Result<MotorDriverWrapper<E1, E2, P1, P2>, MotorDriverError>
359    where
360        E1: OutputPin,
361        E2: OutputPin,
362        P1: SetDutyCycle,
363        P2: SetDutyCycle,
364    {
365        let mut driver = self.build();
366        driver.initialize()?;
367        Ok(driver)
368    }
369}
370
371impl<E1, E2, P1, P2> Default for MotorDriverBuilder<E1, E2, P1, P2> {
372    fn default() -> Self {
373        Self::new()
374    }
375}
376
377#[cfg(feature = "rppal")]
378pub mod rppal {
379    use super::*;
380    use embedded_hal::digital::{OutputPin, InputPin};
381    use embedded_hal::pwm::SetDutyCycle;
382    use ::rppal::gpio::OutputPin as RppalOutputPin;
383    use ::rppal::gpio::InputPin as RppalInputPin;
384    use ::rppal::pwm::Pwm;
385
386    #[derive(Debug)]
387    pub struct RppalError;
388
389    impl embedded_hal::pwm::Error for RppalError {
390        fn kind(&self) -> embedded_hal::pwm::ErrorKind {
391            embedded_hal::pwm::ErrorKind::Other
392        }
393    }
394
395    impl embedded_hal::digital::Error for RppalError {
396        fn kind(&self) -> embedded_hal::digital::ErrorKind {
397            embedded_hal::digital::ErrorKind::Other
398        }
399    }
400
401    pub struct GpioWrapper<P> {
402        pin: P,
403    }
404
405    impl<P> GpioWrapper<P> {
406        pub fn new(pin: P) -> Self {
407            Self { pin }
408        }
409    }
410
411    impl<P> embedded_hal::digital::ErrorType for GpioWrapper<P> {
412        type Error = RppalError;
413    }
414
415    impl OutputPin for GpioWrapper<RppalOutputPin> {
416        fn set_low(&mut self) -> Result<(), Self::Error> {
417            self.pin.set_low();
418            Ok(())
419        }
420
421        fn set_high(&mut self) -> Result<(), Self::Error> {
422            self.pin.set_high();
423            Ok(())
424        }
425    }
426
427    impl InputPin for GpioWrapper<RppalInputPin> {
428        fn is_high(&mut self) -> Result<bool, Self::Error> {
429            Ok(self.pin.is_high())
430        }
431
432        fn is_low(&mut self) -> Result<bool, Self::Error> {
433            Ok(self.pin.is_low())
434        }
435    }
436
437    pub struct PwmWrapper {
438        pwm: Pwm,
439        max_duty: u16,
440    }
441
442    impl PwmWrapper {
443        pub fn new(pwm: Pwm, max_duty: u16) -> Self {
444            Self { pwm, max_duty }
445        }
446    }
447
448    impl embedded_hal::pwm::ErrorType for PwmWrapper {
449        type Error = RppalError;
450    }
451
452    impl SetDutyCycle for PwmWrapper {
453        fn max_duty_cycle(&self) -> u16 {
454            self.max_duty
455        }
456
457        fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
458            let duty_percent = duty as f64 / self.max_duty as f64;
459            self.pwm.set_duty_cycle(duty_percent).map_err(|_| RppalError)?;
460            Ok(())
461        }
462    }
463
464    pub type RppalMotorBuilder = MotorDriverBuilder<
465        GpioWrapper<RppalOutputPin>,
466        GpioWrapper<RppalOutputPin>,
467        PwmWrapper,
468        PwmWrapper
469    >;
470
471    impl RppalMotorBuilder {
472        /// Create a new Raspberry Pi motor wrapper builder.
473        /// 
474        /// # Returns
475        /// 
476        /// A builder for simple motor control using rppal GPIO and PWM.
477        /// 
478        /// # Example
479        /// 
480        /// ```rust
481        /// let motor = RppalMotorBuilder::new_rppal()
482        ///     .with_dual_gpio_enable(&gpio, 23, 24)?
483        ///     .with_pwm_channels(&pwm, 18, 19, 1000.0, 1000)?
484        ///     .build()?;
485        /// ```
486        pub fn new_rppal() -> Self {
487            MotorDriverBuilder::new()
488        }
489
490        pub fn with_gpio_enable(self, gpio: &::rppal::gpio::Gpio, pin: u8) -> Result<Self, ::rppal::gpio::Error> {
491            Ok(self.with_single_enable(GpioWrapper::new(gpio.get(pin)?.into_output())))
492        }
493
494        pub fn with_dual_gpio_enable(self, gpio: &::rppal::gpio::Gpio, pin1: u8, pin2: u8) -> Result<Self, ::rppal::gpio::Error> {
495            Ok(self.with_dual_enable(
496                GpioWrapper::new(gpio.get(pin1)?.into_output()),
497                GpioWrapper::new(gpio.get(pin2)?.into_output())
498            ))
499        }
500
501        pub fn with_pwm_channel(self, channel: ::rppal::pwm::Channel, frequency: f64, max_duty: u16) -> Result<Self, ::rppal::pwm::Error> {
502            let pwm = Pwm::with_frequency(channel, frequency, 0.0, ::rppal::pwm::Polarity::Normal, true)?;
503            Ok(self.with_single_pwm(PwmWrapper::new(pwm, max_duty)))
504        }
505
506        pub fn with_dual_pwm_channels(
507            self, 
508            channel1: ::rppal::pwm::Channel, 
509            channel2: ::rppal::pwm::Channel, 
510            frequency: f64, 
511            max_duty: u16
512        ) -> Result<Self, ::rppal::pwm::Error> {
513            let pwm1 = Pwm::with_frequency(channel1, frequency, 0.0, ::rppal::pwm::Polarity::Normal, true)?;
514            let pwm2 = Pwm::with_frequency(channel2, frequency, 0.0, ::rppal::pwm::Polarity::Normal, true)?;
515            Ok(self.with_dual_pwm(PwmWrapper::new(pwm1, max_duty), PwmWrapper::new(pwm2, max_duty)))
516        }
517    }
518}
519
520#[cfg(feature = "linux-embedded-hal")]
521pub mod linux {
522    use embedded_hal::digital::OutputPin;
523    use embedded_hal::pwm::SetDutyCycle;
524    use linux_embedded_hal::CdevPin;
525
526    #[derive(Debug)]
527    pub struct LinuxError;
528
529    impl embedded_hal::pwm::Error for LinuxError {
530        fn kind(&self) -> embedded_hal::pwm::ErrorKind {
531            embedded_hal::pwm::ErrorKind::Other
532        }
533    }
534
535    impl embedded_hal::digital::Error for LinuxError {
536        fn kind(&self) -> embedded_hal::digital::ErrorKind {
537            embedded_hal::digital::ErrorKind::Other
538        }
539    }
540
541    pub struct GpioWrapper {
542        pin: CdevPin,
543    }
544
545    impl GpioWrapper {
546        pub fn new(pin: CdevPin) -> Self {
547            Self { pin }
548        }
549    }
550
551    impl embedded_hal::digital::ErrorType for GpioWrapper {
552        type Error = LinuxError;
553    }
554
555    impl OutputPin for GpioWrapper {
556        fn set_low(&mut self) -> Result<(), Self::Error> {
557            self.pin.set_value(0).map_err(|_| LinuxError)
558        }
559
560        fn set_high(&mut self) -> Result<(), Self::Error> {
561            self.pin.set_value(1).map_err(|_| LinuxError)
562        }
563    }
564
565    pub struct PwmWrapper {
566        chip: u32,
567        number: u32,
568        max_duty: u16,
569    }
570
571    impl PwmWrapper {
572        pub fn new(chip: u32, number: u32, max_duty: u16) -> Self {
573            Self { chip, number, max_duty }
574        }
575    }
576
577    impl embedded_hal::pwm::ErrorType for PwmWrapper {
578        type Error = LinuxError;
579    }
580
581    impl SetDutyCycle for PwmWrapper {
582        fn max_duty_cycle(&self) -> u16 {
583            self.max_duty
584        }
585
586        fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
587            Ok(())
588        }
589    }
590}
motor_driver_hal/wrapper.rs

motor_driver_hal/
wrapper.rs
