motor_driver_hal/driver.rs
1use crate::{MotorDriver, MotorDriverError};
2use embedded_hal::digital::{OutputPin, InputPin};
3use embedded_hal::pwm::SetDutyCycle;
4
5/// Placeholder encoder implementation for motors without encoder feedback.
6///
7/// This struct provides a no-op implementation of the `InputPin` trait
8/// for use in motor driver configurations that don't require encoder feedback.
9/// It always returns low state and is used as a type parameter placeholder.
10///
11/// # Example
12///
13/// ```rust
14/// use motor_driver_hal::NoEncoder;
15///
16/// // Used automatically when creating drivers without encoders
17/// let motor = HBridgeMotorDriver::single_pwm(enable_pin, pwm_channel, 1000);
18/// ```
19#[derive(Debug)]
20pub struct NoEncoder;
21
22/// Error type for the NoEncoder placeholder implementation.
23///
24/// This error type is never actually returned since NoEncoder operations
25/// always succeed, but it's required to implement the ErrorType trait.
26#[derive(Debug)]
27pub struct NoEncoderError;
28
29impl embedded_hal::digital::Error for NoEncoderError {
30 fn kind(&self) -> embedded_hal::digital::ErrorKind {
31 embedded_hal::digital::ErrorKind::Other
32 }
33}
34
35impl embedded_hal::digital::ErrorType for NoEncoder {
36 type Error = NoEncoderError;
37}
38
39impl InputPin for NoEncoder {
40 fn is_high(&mut self) -> Result<bool, Self::Error> {
41 Ok(false)
42 }
43
44 fn is_low(&mut self) -> Result<bool, Self::Error> {
45 Ok(true)
46 }
47}
48
49#[derive(Copy, Clone, PartialEq)]
50enum Level {
51 Low = 0,
52 High = 1,
53}
54
55/// H-bridge motor driver implementation with optional encoder support.
56///
57/// This struct provides comprehensive motor control functionality including:
58/// - Single or dual PWM channel control for H-bridge motor drivers
59/// - Single or dual enable pin control
60/// - Optional quadrature encoder support for position feedback
61/// - Speed and direction control with safety checks
62///
63/// # Type Parameters
64///
65/// * `E1` - Primary enable pin type implementing `OutputPin`
66/// * `E2` - Secondary enable pin type implementing `OutputPin` (optional)
67/// * `P1` - Primary PWM channel type implementing `SetDutyCycle`
68/// * `P2` - Secondary PWM channel type implementing `SetDutyCycle` (optional)
69/// * `Enc1` - Encoder A channel type implementing `InputPin` (optional)
70/// * `Enc2` - Encoder B channel type implementing `InputPin` (optional)
71///
72/// # Example
73///
74/// ```rust
75/// use motor_driver_hal::HBridgeMotorDriver;
76///
77/// // Create a simple single PWM motor driver
78/// let motor = HBridgeMotorDriver::single_pwm(enable_pin, pwm_channel, 1000);
79///
80/// // Create a dual PWM motor driver with encoders
81/// let motor = HBridgeMotorDriver::dual_pwm_with_encoder(
82/// enable1, enable2, pwm1, pwm2, enc_a, enc_b, 1000
83/// );
84/// ```
85pub struct HBridgeMotorDriver<E1, E2, P1, P2, Enc1, Enc2> {
86 enable1: E1,
87 enable2: Option<E2>,
88 pwm1: P1,
89 pwm2: Option<P2>,
90 encoder1: Option<Enc1>,
91 encoder2: Option<Enc2>,
92 max_duty: u16,
93 current_speed: i16,
94 pulse_count: i32,
95 pulse_offset: i32,
96 target_pulse: i32,
97 ppr: u16,
98 last_enc_a: Level,
99 last_enc_b: Level,
100 direction: bool,
101 initialized: bool,
102}
103
104const QEM: [i8; 16] = [
105 0, -1, 1, 0,
106 1, 0, 0, -1,
107 -1, 0, 0, 1,
108 0, 1, -1, 0,
109];
110
111/// Builder for constructing HBridgeMotorDriver instances.
112///
113/// This builder provides a flexible way to configure motor drivers with
114/// various combinations of enable pins, PWM channels, and encoders.
115///
116/// # Type Parameters
117///
118/// * `E1, E2` - Enable pin types implementing `OutputPin`
119/// * `P1, P2` - PWM channel types implementing `SetDutyCycle`
120/// * `Enc1, Enc2` - Encoder channel types implementing `InputPin`
121///
122/// # Example
123///
124/// ```rust
125/// use motor_driver_hal::HBridgeMotorDriver;
126///
127/// let motor = HBridgeMotorDriver::builder()
128/// .with_enable(enable_pin)
129/// .with_pwm(pwm_channel)
130/// .with_max_duty(1000)
131/// .with_ppr(1024)
132/// .build();
133/// ```
134pub struct HBridgeMotorDriverBuilder<E1, E2, P1, P2, Enc1, Enc2> {
135 enable1: Option<E1>,
136 enable2: Option<E2>,
137 pwm1: Option<P1>,
138 pwm2: Option<P2>,
139 encoder1: Option<Enc1>,
140 encoder2: Option<Enc2>,
141 max_duty: Option<u16>,
142 ppr: Option<u16>,
143 initial_speed: Option<i16>,
144}
145
146impl<E1, E2, P1, P2, Enc1, Enc2> HBridgeMotorDriverBuilder<E1, E2, P1, P2, Enc1, Enc2> {
147 /// Creates a new builder instance with all fields unset.
148 ///
149 /// # Returns
150 ///
151 /// A new `HBridgeMotorDriverBuilder` with default (None) values
152 ///
153 /// # Example
154 ///
155 /// ```rust
156 /// let builder = HBridgeMotorDriverBuilder::new();
157 /// ```
158 pub fn new() -> Self {
159 Self {
160 enable1: None,
161 enable2: None,
162 pwm1: None,
163 pwm2: None,
164 encoder1: None,
165 encoder2: None,
166 max_duty: None,
167 ppr: None,
168 initial_speed: None,
169 }
170 }
171
172 /// Sets the primary enable pin for the motor driver.
173 ///
174 /// # Arguments
175 ///
176 /// * `enable` - GPIO pin implementing `OutputPin` used to enable/disable the motor driver
177 ///
178 /// # Returns
179 ///
180 /// The builder instance for method chaining
181 ///
182 /// # Example
183 ///
184 /// ```rust
185 /// let builder = builder.with_enable(gpio_pin_18);
186 /// ```
187 pub fn with_enable(mut self, enable: E1) -> Self {
188 self.enable1 = Some(enable);
189 self
190 }
191
192 /// Sets both enable pins for dual-enable motor driver configurations.
193 ///
194 /// # Arguments
195 ///
196 /// * `enable1` - Primary enable pin implementing `OutputPin`
197 /// * `enable2` - Secondary enable pin implementing `OutputPin`
198 ///
199 /// # Returns
200 ///
201 /// The builder instance for method chaining
202 ///
203 /// # Example
204 ///
205 /// ```rust
206 /// let builder = builder.with_dual_enable(gpio_pin_18, gpio_pin_19);
207 /// ```
208 pub fn with_dual_enable(mut self, enable1: E1, enable2: E2) -> Self {
209 self.enable1 = Some(enable1);
210 self.enable2 = Some(enable2);
211 self
212 }
213
214 /// Sets the primary PWM channel for motor speed control.
215 ///
216 /// # Arguments
217 ///
218 /// * `pwm` - PWM channel implementing `SetDutyCycle` trait
219 ///
220 /// # Returns
221 ///
222 /// The builder instance for method chaining
223 ///
224 /// # Example
225 ///
226 /// ```rust
227 /// let builder = builder.with_pwm(pwm_channel_0);
228 /// ```
229 pub fn with_pwm(mut self, pwm: P1) -> Self {
230 self.pwm1 = Some(pwm);
231 self
232 }
233
234 /// Sets both PWM channels for dual-PWM motor driver configurations.
235 ///
236 /// This configuration allows for more precise direction control where
237 /// one PWM controls forward motion and the other controls reverse motion.
238 ///
239 /// # Arguments
240 ///
241 /// * `pwm1` - Primary PWM channel (typically forward direction)
242 /// * `pwm2` - Secondary PWM channel (typically reverse direction)
243 ///
244 /// # Returns
245 ///
246 /// The builder instance for method chaining
247 ///
248 /// # Example
249 ///
250 /// ```rust
251 /// let builder = builder.with_dual_pwm(pwm_channel_0, pwm_channel_1);
252 /// ```
253 pub fn with_dual_pwm(mut self, pwm1: P1, pwm2: P2) -> Self {
254 self.pwm1 = Some(pwm1);
255 self.pwm2 = Some(pwm2);
256 self
257 }
258
259 /// Sets the quadrature encoder channels for position feedback.
260 ///
261 /// # Arguments
262 ///
263 /// * `encoder1` - Encoder A channel implementing `InputPin`
264 /// * `encoder2` - Encoder B channel implementing `InputPin`
265 ///
266 /// # Returns
267 ///
268 /// The builder instance for method chaining
269 ///
270 /// # Example
271 ///
272 /// ```rust
273 /// let builder = builder.with_encoder(encoder_a_pin, encoder_b_pin);
274 /// ```
275 pub fn with_encoder(mut self, encoder1: Enc1, encoder2: Enc2) -> Self {
276 self.encoder1 = Some(encoder1);
277 self.encoder2 = Some(encoder2);
278 self
279 }
280
281 /// Sets the maximum duty cycle value for PWM control.
282 ///
283 /// This value determines the resolution and maximum speed of the motor.
284 /// Higher values provide finer speed control resolution.
285 ///
286 /// # Arguments
287 ///
288 /// * `max_duty` - Maximum duty cycle value (typical values: 255, 1000, 4095)
289 ///
290 /// # Returns
291 ///
292 /// The builder instance for method chaining
293 ///
294 /// # Example
295 ///
296 /// ```rust
297 /// let builder = builder.with_max_duty(1000); // 0-1000 speed range
298 /// ```
299 pub fn with_max_duty(mut self, max_duty: u16) -> Self {
300 self.max_duty = Some(max_duty);
301 self
302 }
303
304 /// Sets the pulses per revolution for encoder calculations.
305 ///
306 /// This value is used for position control and speed calculations
307 /// when encoders are present.
308 ///
309 /// # Arguments
310 ///
311 /// * `ppr` - Number of encoder pulses per complete motor revolution
312 ///
313 /// # Returns
314 ///
315 /// The builder instance for method chaining
316 ///
317 /// # Example
318 ///
319 /// ```rust
320 /// let builder = builder.with_ppr(1024); // 1024 pulses per revolution
321 /// ```
322 pub fn with_ppr(mut self, ppr: u16) -> Self {
323 self.ppr = Some(ppr);
324 self
325 }
326
327 /// Sets the initial speed value for the motor driver.
328 ///
329 /// The motor will be configured to this speed when built, but will
330 /// not actually move until `enable()` is called.
331 ///
332 /// # Arguments
333 ///
334 /// * `speed` - Initial speed value (positive = forward, negative = reverse)
335 ///
336 /// # Returns
337 ///
338 /// The builder instance for method chaining
339 ///
340 /// # Example
341 ///
342 /// ```rust
343 /// let builder = builder.with_initial_speed(0); // Start stopped
344 /// ```
345 pub fn with_initial_speed(mut self, speed: i16) -> Self {
346 self.initial_speed = Some(speed);
347 self
348 }
349
350 /// Builds the motor driver instance from the configured parameters.
351 ///
352 /// # Returns
353 ///
354 /// A configured `HBridgeMotorDriver` instance ready for initialization
355 ///
356 /// # Panics
357 ///
358 /// Panics if required parameters (enable pin and PWM channel) are not set
359 ///
360 /// # Example
361 ///
362 /// ```rust
363 /// let motor = HBridgeMotorDriver::builder()
364 /// .with_enable(enable_pin)
365 /// .with_pwm(pwm_channel)
366 /// .build();
367 /// ```
368 pub fn build(self) -> HBridgeMotorDriver<E1, E2, P1, P2, Enc1, Enc2> {
369 HBridgeMotorDriver {
370 enable1: self.enable1.expect("Enable pin is required"),
371 enable2: self.enable2,
372 pwm1: self.pwm1.expect("PWM channel is required"),
373 pwm2: self.pwm2,
374 encoder1: self.encoder1,
375 encoder2: self.encoder2,
376 max_duty: self.max_duty.unwrap_or(1000),
377 current_speed: self.initial_speed.unwrap_or(0),
378 pulse_count: 0,
379 pulse_offset: 0,
380 target_pulse: 0,
381 ppr: self.ppr.unwrap_or(0),
382 last_enc_a: Level::Low,
383 last_enc_b: Level::Low,
384 direction: true,
385 initialized: false,
386 }
387 }
388
389 /// Builds and initializes the motor driver in one step.
390 ///
391 /// This convenience method combines `build()` and `initialize()` operations,
392 /// returning a ready-to-use motor driver instance.
393 ///
394 /// # Returns
395 ///
396 /// * `Ok(driver)` - Initialized motor driver ready for use
397 /// * `Err(MotorDriverError)` - If building fails or initialization fails
398 ///
399 /// # Errors
400 ///
401 /// Returns error if required parameters are missing or hardware initialization fails.
402 ///
403 /// # Example
404 ///
405 /// ```rust
406 /// let motor = HBridgeMotorDriver::builder()
407 /// .with_enable(enable_pin)
408 /// .with_pwm(pwm_channel)
409 /// .build_and_init()?;
410 /// ```
411 pub fn build_and_init(self) -> Result<HBridgeMotorDriver<E1, E2, P1, P2, Enc1, Enc2>, MotorDriverError>
412 where
413 E1: OutputPin,
414 E2: OutputPin,
415 P1: SetDutyCycle,
416 P2: SetDutyCycle,
417 Enc1: InputPin,
418 Enc2: InputPin,
419 {
420 let mut driver = self.build();
421 driver.initialize()?;
422 Ok(driver)
423 }
424}
425
426impl<E1, E2, P1, P2> HBridgeMotorDriver<E1, E2, P1, P2, NoEncoder, NoEncoder>
427where
428 E1: OutputPin,
429 E2: OutputPin,
430 P1: SetDutyCycle,
431 P2: SetDutyCycle,
432{
433 /// Creates a new builder for motor drivers without encoder support.
434 ///
435 /// # Returns
436 ///
437 /// A new builder instance configured for NoEncoder types
438 ///
439 /// # Example
440 ///
441 /// ```rust
442 /// let motor = HBridgeMotorDriver::builder()
443 /// .with_enable(enable_pin)
444 /// .with_pwm(pwm_channel)
445 /// .build();
446 /// ```
447 pub fn builder() -> HBridgeMotorDriverBuilder<E1, E2, P1, P2, NoEncoder, NoEncoder> {
448 HBridgeMotorDriverBuilder::new()
449 }
450
451 /// Creates a motor driver with single PWM channel configuration.
452 ///
453 /// This is a convenience constructor for the most common motor driver
454 /// configuration using one enable pin and one PWM channel.
455 ///
456 /// # Arguments
457 ///
458 /// * `enable` - GPIO pin for enabling/disabling the motor driver
459 /// * `pwm` - PWM channel for speed control
460 /// * `max_duty` - Maximum duty cycle value for speed scaling
461 ///
462 /// # Returns
463 ///
464 /// A configured motor driver instance (not yet initialized)
465 ///
466 /// # Example
467 ///
468 /// ```rust
469 /// let motor = HBridgeMotorDriver::single_pwm(enable_pin, pwm_channel, 1000);
470 /// ```
471 pub fn single_pwm(enable: E1, pwm: P1, max_duty: u16) -> Self {
472 Self::builder()
473 .with_enable(enable)
474 .with_pwm(pwm)
475 .with_max_duty(max_duty)
476 .build()
477 }
478
479 /// Creates a motor driver with dual PWM and dual enable configuration.
480 ///
481 /// This configuration provides the most control options with separate
482 /// PWM channels for each direction and separate enable pins.
483 ///
484 /// # Arguments
485 ///
486 /// * `enable1` - Primary enable pin
487 /// * `enable2` - Secondary enable pin
488 /// * `pwm1` - Primary PWM channel (forward direction)
489 /// * `pwm2` - Secondary PWM channel (reverse direction)
490 /// * `max_duty` - Maximum duty cycle value for both PWM channels
491 ///
492 /// # Returns
493 ///
494 /// A configured motor driver instance (not yet initialized)
495 ///
496 /// # Example
497 ///
498 /// ```rust
499 /// let motor = HBridgeMotorDriver::dual_pwm(
500 /// enable1, enable2, pwm1, pwm2, 1000
501 /// );
502 /// ```
503 pub fn dual_pwm(enable1: E1, enable2: E2, pwm1: P1, pwm2: P2, max_duty: u16) -> Self {
504 Self::builder()
505 .with_dual_enable(enable1, enable2)
506 .with_dual_pwm(pwm1, pwm2)
507 .with_max_duty(max_duty)
508 .build()
509 }
510}
511
512impl<E1, E2, P1, P2, Enc1, Enc2> HBridgeMotorDriver<E1, E2, P1, P2, Enc1, Enc2>
513where
514 E1: OutputPin,
515 E2: OutputPin,
516 P1: SetDutyCycle,
517 P2: SetDutyCycle,
518 Enc1: InputPin,
519 Enc2: InputPin,
520{
521 /// Creates a new builder for motor drivers with encoder support.
522 ///
523 /// # Returns
524 ///
525 /// A new builder instance configured for encoder types Enc1 and Enc2
526 ///
527 /// # Example
528 ///
529 /// ```rust
530 /// let motor = HBridgeMotorDriver::builder_with_encoder()
531 /// .with_dual_enable(enable1, enable2)
532 /// .with_dual_pwm(pwm1, pwm2)
533 /// .with_encoder(enc_a, enc_b)
534 /// .with_ppr(1024)
535 /// .build();
536 /// ```
537 pub fn builder_with_encoder() -> HBridgeMotorDriverBuilder<E1, E2, P1, P2, Enc1, Enc2> {
538 HBridgeMotorDriverBuilder::new()
539 }
540
541 /// Creates a motor driver with dual PWM, dual enable, and encoder support.
542 ///
543 /// This is the most feature-complete configuration providing precise
544 /// motor control with position feedback.
545 ///
546 /// # Arguments
547 ///
548 /// * `enable1` - Primary enable pin
549 /// * `enable2` - Secondary enable pin
550 /// * `pwm1` - Primary PWM channel (forward direction)
551 /// * `pwm2` - Secondary PWM channel (reverse direction)
552 /// * `encoder1` - Encoder A channel pin
553 /// * `encoder2` - Encoder B channel pin
554 /// * `max_duty` - Maximum duty cycle value
555 ///
556 /// # Returns
557 ///
558 /// A configured motor driver instance with encoder support
559 ///
560 /// # Example
561 ///
562 /// ```rust
563 /// let motor = HBridgeMotorDriver::dual_pwm_with_encoder(
564 /// enable1, enable2, pwm1, pwm2, enc_a, enc_b, 1000
565 /// );
566 /// ```
567 pub fn dual_pwm_with_encoder(enable1: E1, enable2: E2, pwm1: P1, pwm2: P2, encoder1: Enc1, encoder2: Enc2, max_duty: u16) -> Self {
568 Self::builder_with_encoder()
569 .with_dual_enable(enable1, enable2)
570 .with_dual_pwm(pwm1, pwm2)
571 .with_encoder(encoder1, encoder2)
572 .with_max_duty(max_duty)
573 .build()
574 }
575
576 fn update_pwm(&mut self) -> Result<(), MotorDriverError> {
577 let duty = if self.current_speed < 0 {
578 (-self.current_speed as u16).min(self.max_duty)
579 } else {
580 (self.current_speed as u16).min(self.max_duty)
581 };
582
583 match (&mut self.pwm2, self.direction) {
584 (Some(pwm2), true) => {
585 self.pwm1.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
586 pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
587 }
588 (Some(pwm2), false) => {
589 self.pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
590 pwm2.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
591 }
592 (None, _) => {
593 self.pwm1.set_duty_cycle(duty).map_err(|_| MotorDriverError::PwmError)?;
594 }
595 }
596 Ok(())
597 }
598
599 /// Reads the current encoder state and updates pulse count.
600 ///
601 /// This method implements quadrature encoder decoding using a state machine
602 /// to track motor position. It should be called regularly (typically in a
603 /// timer interrupt or polling loop) to maintain accurate position tracking.
604 ///
605 /// # Returns
606 ///
607 /// * `Ok(())` if encoder reading succeeds
608 /// * `Err(MotorDriverError::GpioError)` if encoder pin reading fails
609 /// * `Err(MotorDriverError::HardwareFault)` if encoders are not configured
610 ///
611 /// # Example
612 ///
613 /// ```rust
614 /// // In a timer interrupt or polling loop
615 /// motor.read_encoder()?;
616 /// let position = motor.get_pulse_count();
617 /// ```
618 pub fn read_encoder(&mut self) -> Result<(), MotorDriverError> {
619 if let (Some(ref mut enc_a), Some(ref mut enc_b)) = (&mut self.encoder1, &mut self.encoder2) {
620 let level_a = if enc_a.is_high().map_err(|_| MotorDriverError::GpioError)? {
621 Level::High
622 } else {
623 Level::Low
624 };
625 let level_b = if enc_b.is_high().map_err(|_| MotorDriverError::GpioError)? {
626 Level::High
627 } else {
628 Level::Low
629 };
630
631 let index = ((self.last_enc_a as u8) << 3)
632 | ((self.last_enc_b as u8) << 2)
633 | ((level_a as u8) << 1)
634 | (level_b as u8);
635
636 self.pulse_count += QEM[index as usize] as i32;
637 self.last_enc_a = level_a;
638 self.last_enc_b = level_b;
639
640 Ok(())
641 } else {
642 Err(MotorDriverError::HardwareFault)
643 }
644 }
645
646 /// Gets the current encoder pulse count relative to the last reset.
647 ///
648 /// The pulse count is automatically adjusted by the pulse offset set
649 /// by `reset_encoder()` to provide relative position measurements.
650 ///
651 /// # Returns
652 ///
653 /// Current pulse count since last encoder reset
654 ///
655 /// # Example
656 ///
657 /// ```rust
658 /// motor.reset_encoder(); // Reset to zero
659 /// // ... motor movement ...
660 /// let position = motor.get_pulse_count(); // Position since reset
661 /// ```
662 pub fn get_pulse_count(&self) -> i32 {
663 self.pulse_count - self.pulse_offset
664 }
665
666 /// Resets the encoder position counter to zero.
667 ///
668 /// This sets the current position as the new reference point (zero).
669 /// Subsequent calls to `get_pulse_count()` will return values relative
670 /// to this reset point.
671 ///
672 /// # Example
673 ///
674 /// ```rust
675 /// motor.reset_encoder(); // Set current position as zero
676 /// ```
677 pub fn reset_encoder(&mut self) {
678 self.pulse_offset = self.pulse_count;
679 }
680
681 /// Sets the target pulse count for position control.
682 ///
683 /// This target is used by `check_ppr()` to verify that the motor
684 /// has reached the desired position.
685 ///
686 /// # Arguments
687 ///
688 /// * `target` - Target pulse count relative to encoder reset point
689 ///
690 /// # Example
691 ///
692 /// ```rust
693 /// motor.set_target_pulse(1000); // Move 1000 pulses from current position
694 /// ```
695 pub fn set_target_pulse(&mut self, target: i32) {
696 self.target_pulse = target;
697 }
698}
699
700impl<E1, E2, P1, P2, Enc1, Enc2> MotorDriver for HBridgeMotorDriver<E1, E2, P1, P2, Enc1, Enc2>
701where
702 E1: OutputPin,
703 E2: OutputPin,
704 P1: SetDutyCycle,
705 P2: SetDutyCycle,
706 Enc1: InputPin,
707 Enc2: InputPin,
708{
709 type Error = MotorDriverError;
710
711 fn initialize(&mut self) -> Result<(), Self::Error> {
712 self.enable1.set_low().map_err(|_| MotorDriverError::GpioError)?;
713 if let Some(ref mut enable2) = self.enable2 {
714 enable2.set_low().map_err(|_| MotorDriverError::GpioError)?;
715 }
716
717 self.pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
718 if let Some(ref mut pwm2) = self.pwm2 {
719 pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
720 }
721
722 self.initialized = true;
723 Ok(())
724 }
725
726 fn set_speed(&mut self, speed: i16) -> Result<(), Self::Error> {
727 if !self.initialized {
728 return Err(MotorDriverError::NotInitialized);
729 }
730
731 if speed.unsigned_abs() > self.max_duty {
732 return Err(MotorDriverError::InvalidSpeed);
733 }
734
735 self.current_speed = speed;
736 self.direction = speed >= 0;
737
738 self.update_pwm()
739 }
740
741 fn set_direction(&mut self, forward: bool) -> Result<(), Self::Error> {
742 if !self.initialized {
743 return Err(MotorDriverError::NotInitialized);
744 }
745
746 self.direction = forward;
747 self.update_pwm()
748 }
749
750 fn stop(&mut self) -> Result<(), Self::Error> {
751 if !self.initialized {
752 return Err(MotorDriverError::NotInitialized);
753 }
754
755 self.current_speed = 0;
756 self.pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
757 if let Some(ref mut pwm2) = self.pwm2 {
758 pwm2.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
759 }
760 Ok(())
761 }
762
763 fn brake(&mut self) -> Result<(), Self::Error> {
764 if !self.initialized {
765 return Err(MotorDriverError::NotInitialized);
766 }
767
768 self.current_speed = 0;
769 if let Some(ref mut pwm2) = self.pwm2 {
770 self.pwm1.set_duty_cycle(self.max_duty).map_err(|_| MotorDriverError::PwmError)?;
771 pwm2.set_duty_cycle(self.max_duty).map_err(|_| MotorDriverError::PwmError)?;
772 } else {
773 self.pwm1.set_duty_cycle(0).map_err(|_| MotorDriverError::PwmError)?;
774 }
775 Ok(())
776 }
777
778 fn enable(&mut self) -> Result<(), Self::Error> {
779 if !self.initialized {
780 return Err(MotorDriverError::NotInitialized);
781 }
782
783 self.enable1.set_high().map_err(|_| MotorDriverError::GpioError)?;
784 if let Some(ref mut enable2) = self.enable2 {
785 enable2.set_high().map_err(|_| MotorDriverError::GpioError)?;
786 }
787 Ok(())
788 }
789
790 fn disable(&mut self) -> Result<(), Self::Error> {
791 if !self.initialized {
792 return Err(MotorDriverError::NotInitialized);
793 }
794
795 self.enable1.set_low().map_err(|_| MotorDriverError::GpioError)?;
796 if let Some(ref mut enable2) = self.enable2 {
797 enable2.set_low().map_err(|_| MotorDriverError::GpioError)?;
798 }
799 Ok(())
800 }
801
802 fn get_speed(&self) -> Result<i16, Self::Error> {
803 if !self.initialized {
804 return Err(MotorDriverError::NotInitialized);
805 }
806 Ok(self.current_speed)
807 }
808
809 fn get_direction(&self) -> Result<bool, Self::Error> {
810 if !self.initialized {
811 return Err(MotorDriverError::NotInitialized);
812 }
813 Ok(self.direction)
814 }
815
816 fn set_ppr(&mut self, ppr: i16) -> Result<bool, Self::Error> {
817 if !self.initialized {
818 return Err(MotorDriverError::NotInitialized);
819 }
820 if ppr <= 0 {
821 return Err(MotorDriverError::InvalidSpeed);
822 }
823 self.ppr = ppr as u16;
824 Ok(true)
825 }
826
827 fn check_ppr(&mut self) -> Result<(), Self::Error> {
828 if self.ppr == 0 {
829 return Err(MotorDriverError::NotInitialized);
830 }
831
832 self.read_encoder()?;
833
834 let current_pulse = self.get_pulse_count();
835 let current_rotation_pulse = current_pulse % (self.ppr as i32);
836 let target_rotation_pulse = self.target_pulse % (self.ppr as i32);
837
838 if current_rotation_pulse == target_rotation_pulse {
839 Ok(())
840 } else {
841 Err(MotorDriverError::HardwareFault)
842 }
843 }
844
845
846 fn get_current(&self) -> Result<f32, Self::Error> {
847 Err(MotorDriverError::HardwareFault)
848 }
849
850 fn get_voltage(&self) -> Result<f32, Self::Error> {
851 Err(MotorDriverError::HardwareFault)
852 }
853
854 fn get_temperature(&self) -> Result<f32, Self::Error> {
855 Err(MotorDriverError::HardwareFault)
856 }
857
858 fn get_fault_status(&self) -> Result<u8, Self::Error> {
859 if !self.initialized {
860 return Err(MotorDriverError::NotInitialized);
861 }
862 Ok(0)
863 }
864}
865
866#[cfg(feature = "rppal")]
867pub mod rppal {
868 use super::*;
869 use crate::wrapper::rppal::{GpioWrapper, PwmWrapper};
870 use ::rppal::gpio::{Gpio, InputPin as RppalInputPin, OutputPin as RppalOutputPin};
871 use ::rppal::pwm::{Channel, Pwm, Polarity};
872
873 pub type RppalMotorDriverBuilder = HBridgeMotorDriverBuilder<
874 GpioWrapper<RppalOutputPin>,
875 GpioWrapper<RppalOutputPin>,
876 PwmWrapper,
877 PwmWrapper,
878 GpioWrapper<RppalInputPin>,
879 GpioWrapper<RppalInputPin>
880 >;
881
882 impl RppalMotorDriverBuilder {
883 /// Create a new Raspberry Pi motor driver builder.
884 ///
885 /// # Returns
886 ///
887 /// A builder configured for use with rppal GPIO and PWM.
888 ///
889 /// # Example
890 ///
891 /// ```rust
892 /// let motor = RppalMotorDriverBuilder::new_rppal()
893 /// .with_dual_gpio_enable(&gpio, 23, 24)?
894 /// .with_dual_pwm_channels(Channel::Pwm1, Channel::Pwm2, 1000.0, 1000)?
895 /// .build_and_init()?;
896 /// ```
897 pub fn new_rppal() -> Self {
898 HBridgeMotorDriverBuilder::new()
899 }
900
901 pub fn with_gpio_enable(mut self, gpio: &Gpio, pin: u8) -> Result<Self, ::rppal::gpio::Error> {
902 self.enable1 = Some(GpioWrapper::new(gpio.get(pin)?.into_output()));
903 Ok(self)
904 }
905
906 /// Configure dual GPIO enable pins for H-bridge control.
907 ///
908 /// # Arguments
909 ///
910 /// * `gpio` - Raspberry Pi GPIO interface
911 /// * `pin1` - First enable pin number (0-27)
912 /// * `pin2` - Second enable pin number (0-27)
913 ///
914 /// # Example
915 ///
916 /// ```rust
917 /// builder.with_dual_gpio_enable(&gpio, 23, 24)?
918 /// ```
919 pub fn with_dual_gpio_enable(mut self, gpio: &Gpio, pin1: u8, pin2: u8) -> Result<Self, ::rppal::gpio::Error> {
920 self.enable1 = Some(GpioWrapper::new(gpio.get(pin1)?.into_output()));
921 self.enable2 = Some(GpioWrapper::new(gpio.get(pin2)?.into_output()));
922 Ok(self)
923 }
924
925 pub fn with_pwm_channel(mut self, channel: Channel, frequency: f64, max_duty: u16) -> Result<Self, ::rppal::pwm::Error> {
926 let pwm = Pwm::with_frequency(channel, frequency, 0.0, Polarity::Normal, true)?;
927 self.pwm1 = Some(PwmWrapper::new(pwm, max_duty));
928 self.max_duty = Some(max_duty);
929 Ok(self)
930 }
931
932 /// Configure dual PWM channels for motor speed control.
933 ///
934 /// # Arguments
935 ///
936 /// * `channel1` - First PWM channel (Channel::Pwm1 or Channel::Pwm2)
937 /// * `channel2` - Second PWM channel (Channel::Pwm1 or Channel::Pwm2)
938 /// * `frequency` - PWM frequency in Hz (e.g., 1000.0)
939 /// * `max_duty` - Maximum duty cycle value (e.g., 1000)
940 ///
941 /// # Example
942 ///
943 /// ```rust
944 /// builder.with_dual_pwm_channels(Channel::Pwm1, Channel::Pwm2, 1000.0, 1000)?
945 /// ```
946 pub fn with_dual_pwm_channels(
947 mut self,
948 channel1: Channel,
949 channel2: Channel,
950 frequency: f64,
951 max_duty: u16
952 ) -> Result<Self, ::rppal::pwm::Error> {
953 let pwm1 = Pwm::with_frequency(channel1, frequency, 0.0, Polarity::Normal, true)?;
954 let pwm2 = Pwm::with_frequency(channel2, frequency, 0.0, Polarity::Normal, true)?;
955 self.pwm1 = Some(PwmWrapper::new(pwm1, max_duty));
956 self.pwm2 = Some(PwmWrapper::new(pwm2, max_duty));
957 self.max_duty = Some(max_duty);
958 Ok(self)
959 }
960
961 /// Configure quadrature encoder pins for position feedback.
962 ///
963 /// # Arguments
964 ///
965 /// * `gpio` - Raspberry Pi GPIO interface
966 /// * `pin_a` - Encoder A phase pin number (0-27)
967 /// * `pin_b` - Encoder B phase pin number (0-27)
968 ///
969 /// # Example
970 ///
971 /// ```rust
972 /// builder.with_encoder_pins(&gpio, 25, 8)?
973 /// ```
974 pub fn with_encoder_pins(mut self, gpio: &Gpio, pin_a: u8, pin_b: u8) -> Result<Self, ::rppal::gpio::Error> {
975 self.encoder1 = Some(GpioWrapper::new(gpio.get(pin_a)?.into_input_pullup()));
976 self.encoder2 = Some(GpioWrapper::new(gpio.get(pin_b)?.into_input_pullup()));
977 Ok(self)
978 }
979 }
980}
981
982#[cfg(feature = "linux-embedded-hal")]
983pub mod linux {
984 use super::*;
985 use crate::wrapper::linux::{GpioWrapper, PwmWrapper};
986 use linux_embedded_hal::{gpio_cdev::Chip, CdevPin};
987
988 pub type LinuxMotorDriverBuilder = HBridgeMotorDriverBuilder<
989 GpioWrapper,
990 GpioWrapper,
991 PwmWrapper,
992 PwmWrapper,
993 NoEncoder,
994 NoEncoder
995 >;
996
997 impl LinuxMotorDriverBuilder {
998 /// Create a new Linux motor driver builder.
999 ///
1000 /// # Returns
1001 ///
1002 /// A builder configured for use with linux-embedded-hal GPIO and PWM.
1003 ///
1004 /// # Example
1005 ///
1006 /// ```rust
1007 /// let motor = LinuxMotorDriverBuilder::new_linux()
1008 /// .with_dual_gpio_enable(&mut chip, 23, 24)?
1009 /// .with_dual_pwm_channels(0, 0, 1, 1000)
1010 /// .build_and_init()?;
1011 /// ```
1012 pub fn new_linux() -> Self {
1013 HBridgeMotorDriverBuilder::new()
1014 }
1015
1016 pub fn with_gpio_enable(mut self, chip: &mut Chip, pin: u32) -> Result<Self, linux_embedded_hal::gpio_cdev::errors::Error> {
1017 let handle = chip.get_line(pin)?.request(
1018 linux_embedded_hal::gpio_cdev::LineRequestFlags::OUTPUT,
1019 0,
1020 "enable"
1021 )?;
1022 self.enable1 = Some(GpioWrapper::new(CdevPin::new(handle)?));
1023 Ok(self)
1024 }
1025
1026 pub fn with_dual_gpio_enable(mut self, chip: &mut Chip, pin1: u32, pin2: u32) -> Result<Self, linux_embedded_hal::gpio_cdev::errors::Error> {
1027 let handle1 = chip.get_line(pin1)?.request(
1028 linux_embedded_hal::gpio_cdev::LineRequestFlags::OUTPUT,
1029 0,
1030 "enable1"
1031 )?;
1032 let handle2 = chip.get_line(pin2)?.request(
1033 linux_embedded_hal::gpio_cdev::LineRequestFlags::OUTPUT,
1034 0,
1035 "enable2"
1036 )?;
1037 self.enable1 = Some(GpioWrapper::new(CdevPin::new(handle1)?));
1038 self.enable2 = Some(GpioWrapper::new(CdevPin::new(handle2)?));
1039 Ok(self)
1040 }
1041
1042 pub fn with_pwm_channel(mut self, chip: u32, channel: u32, max_duty: u16) -> Self {
1043 self.pwm1 = Some(PwmWrapper::new(chip, channel, max_duty));
1044 self.max_duty = Some(max_duty);
1045 self
1046 }
1047
1048 pub fn with_dual_pwm_channels(mut self, chip: u32, channel1: u32, channel2: u32, max_duty: u16) -> Self {
1049 self.pwm1 = Some(PwmWrapper::new(chip, channel1, max_duty));
1050 self.pwm2 = Some(PwmWrapper::new(chip, channel2, max_duty));
1051 self.max_duty = Some(max_duty);
1052 self
1053 }
1054 }
1055}
1056