omnitiles/drivers/
fit0185.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Motor abstraction for DFRobot FIT0185 motor with DRV8873 SPI driver and TIM2 quadrature encoder.
5//!
6//! This module includes functions to drive the motor and read encoder values.
7
8use crate::drivers::drv8873::{Diag, Drv8873, Fault};
9use crate::hw::spi::CsControl;
10use crate::hw::{Encoder, SpiBus};
11
12use micromath::F32Ext;
13
14use stm32f7xx_hal::{
15    gpio::{self, Output, PushPull},
16    pac, spi,
17};
18
19/// Logical drive direction / mode for the H-bridge.
20#[derive(Copy, Clone, Debug)]
21pub enum Direction {
22    Forward,
23    Reverse,
24    Brake,
25    Coast,
26}
27
28/// Motor abstraction that combines a DRV8873 driver, four control pins, and a TIM2 encoder.
29pub struct Fit0185<
30    CS: CsControl,
31    const IN1_P: char,
32    const IN1_N: u8,
33    const IN2_P: char,
34    const IN2_N: u8,
35    const SLP_P: char,
36    const SLP_N: u8,
37    const DIS_P: char,
38    const DIS_N: u8,
39> {
40    drv: Drv8873<CS>,
41    enc: Encoder<pac::TIM2>,
42    in1: gpio::Pin<IN1_P, IN1_N, Output<PushPull>>,
43    in2: gpio::Pin<IN2_P, IN2_N, Output<PushPull>>,
44    nsleep: gpio::Pin<SLP_P, SLP_N, Output<PushPull>>,
45    disable: gpio::Pin<DIS_P, DIS_N, Output<PushPull>>,
46    counts_per_rev: u32,
47}
48
49impl<
50        CS: CsControl,
51        const IN1_P: char,
52        const IN1_N: u8,
53        const IN2_P: char,
54        const IN2_N: u8,
55        const SLP_P: char,
56        const SLP_N: u8,
57        const DIS_P: char,
58        const DIS_N: u8,
59    > Fit0185<CS, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N>
60{
61    /// Construct a new `SpiMotor`.
62    ///
63    /// `counts_per_rev` is the encoder resolution at the mechanical shaft (after any gear ratio).
64    pub fn new<In1Mode, In2Mode, SlpMode, DisMode>(
65        drv: Drv8873<CS>,
66        enc: Encoder<pac::TIM2>,
67        in1: gpio::Pin<IN1_P, IN1_N, In1Mode>,
68        in2: gpio::Pin<IN2_P, IN2_N, In2Mode>,
69        nsleep: gpio::Pin<SLP_P, SLP_N, SlpMode>,
70        disable: gpio::Pin<DIS_P, DIS_N, DisMode>,
71        counts_per_rev: u32,
72    ) -> Self {
73        let mut in1 = in1.into_push_pull_output();
74        let mut in2 = in2.into_push_pull_output();
75        let mut nsleep = nsleep.into_push_pull_output();
76        let mut disable = disable.into_push_pull_output();
77
78        in1.set_low();
79        in2.set_low();
80
81        // Initialize as awake + disabled
82        nsleep.set_high();
83        disable.set_high();
84
85        Self {
86            drv,
87            enc,
88            in1,
89            in2,
90            nsleep,
91            disable,
92            counts_per_rev,
93        }
94    }
95
96    /// Tear down this motor and return its constituent parts.
97    pub fn free(
98        self,
99    ) -> (
100        Drv8873<CS>,
101        Encoder<pac::TIM2>,
102        gpio::Pin<IN1_P, IN1_N, Output<PushPull>>,
103        gpio::Pin<IN2_P, IN2_N, Output<PushPull>>,
104        gpio::Pin<SLP_P, SLP_N, Output<PushPull>>,
105        gpio::Pin<DIS_P, DIS_N, Output<PushPull>>,
106    ) {
107        (
108            self.drv,
109            self.enc,
110            self.in1,
111            self.in2,
112            self.nsleep,
113            self.disable,
114        )
115    }
116
117    /// Initialize and set base configuration for the DRV8873.
118    pub fn init<I, PINS>(&mut self, spi_bus: &mut SpiBus<I, PINS>) -> Result<(), spi::Error>
119    where
120        I: spi::Instance,
121        PINS: spi::Pins<I>,
122    {
123        let _fault = self.drv.read_fault(spi_bus)?;
124        let _diag = self.drv.read_diag(spi_bus)?;
125
126        Ok(())
127    }
128
129    /// Read the FAULT status register.
130    #[inline]
131    pub fn read_fault<I, PINS>(
132        &mut self,
133        spi_bus: &mut SpiBus<I, PINS>,
134    ) -> Result<Fault, spi::Error>
135    where
136        I: spi::Instance,
137        PINS: spi::Pins<I>,
138    {
139        self.drv.read_fault(spi_bus)
140    }
141
142    /// Read the DIAG status register.
143    #[inline]
144    pub fn read_diag<I, PINS>(&mut self, spi_bus: &mut SpiBus<I, PINS>) -> Result<Diag, spi::Error>
145    where
146        I: spi::Instance,
147        PINS: spi::Pins<I>,
148    {
149        self.drv.read_diag(spi_bus)
150    }
151
152    /// Access the underlying DRV8873 driver for advanced SPI control.
153    #[inline]
154    pub fn drv(&mut self) -> &mut Drv8873<CS> {
155        &mut self.drv
156    }
157
158    /// Put the driver into sleep mode.
159    ///
160    /// This shuts down most of the internal circuitry to reduce power consumption.
161    #[inline]
162    pub fn sleep(&mut self) {
163        self.nsleep.set_low();
164    }
165
166    /// Wake the driver from sleep mode.
167    #[inline]
168    pub fn wake(&mut self) {
169        self.nsleep.set_high();
170    }
171
172    /// Enable the motor and wake the driver if in sleep.
173    #[inline]
174    pub fn enable_outputs(&mut self) {
175        self.wake();
176        self.disable.set_low();
177    }
178
179    /// Disable the motor and coast.
180    #[inline]
181    pub fn disable_outputs(&mut self) {
182        self.set_direction_pins(Direction::Coast);
183        self.disable.set_high();
184    }
185
186    /// Configure IN1/IN2 pins for a given direction/mode.
187    fn set_direction_pins(&mut self, dir: Direction) {
188        match dir {
189            Direction::Forward => {
190                self.in1.set_high();
191                self.in2.set_low();
192            }
193            Direction::Reverse => {
194                self.in1.set_low();
195                self.in2.set_high();
196            }
197            Direction::Brake => {
198                self.in1.set_high();
199                self.in2.set_high();
200            }
201            Direction::Coast => {
202                self.in1.set_low();
203                self.in2.set_low();
204            }
205        }
206    }
207
208    /// Drive the motor forward.
209    #[inline]
210    pub fn forward(&mut self) {
211        self.set_direction_pins(Direction::Forward);
212    }
213
214    /// Drive the motor in reverse.
215    #[inline]
216    pub fn reverse(&mut self) {
217        self.set_direction_pins(Direction::Reverse);
218    }
219
220    /// Apply brakes.
221    #[inline]
222    pub fn brake(&mut self) {
223        self.set_direction_pins(Direction::Brake);
224    }
225
226    /// Coast (this sets outputs to HiZ).
227    #[inline]
228    pub fn coast(&mut self) {
229        self.set_direction_pins(Direction::Coast);
230    }
231
232    /// Raw encoder position in ticks (signed).
233    #[inline]
234    pub fn position_ticks(&self) -> i32 {
235        self.enc.position()
236    }
237
238    /// Encoder position converted to revolutions.
239    #[inline]
240    pub fn position_revs(&self) -> f32 {
241        self.enc.position() as f32 / self.counts_per_rev as f32
242    }
243
244    /// Reset the encoder position to zero.
245    #[inline]
246    pub fn zero(&mut self) {
247        self.enc.reset();
248    }
249
250    /// Convert a number of revolutions into encoder ticks.
251    #[inline]
252    pub fn ticks_for_revs(&self, revs: f32) -> i32 {
253        (revs * self.counts_per_rev as f32).round() as i32
254    }
255
256    /// Compute a target tick position for a relative move from the current position.
257    #[inline]
258    pub fn target_for_delta_ticks(&self, delta_ticks: i32) -> i32 {
259        self.position_ticks().wrapping_add(delta_ticks)
260    }
261
262    /// Compute a target tick position for a relative move in revolutions.
263    #[inline]
264    pub fn target_for_delta_revs(&self, delta_revs: f32) -> i32 {
265        let delta_ticks = self.ticks_for_revs(delta_revs);
266        self.target_for_delta_ticks(delta_ticks)
267    }
268
269    /// Expose the underlying encoder.
270    #[inline]
271    pub fn encoder(&self) -> &Encoder<pac::TIM2> {
272        &self.enc
273    }
274
275    /// Mutable access to the encoder.
276    #[inline]
277    pub fn encoder_mut(&mut self) -> &mut Encoder<pac::TIM2> {
278        &mut self.enc
279    }
280
281    /// Apply PID output.
282    pub fn apply_pid_output(&mut self, u: f32) {
283        if u > 0.0 {
284            self.forward();
285        } else if u < 0.0 {
286            self.reverse();
287        } else {
288            self.coast();
289        }
290    }
291}
omnitiles/drivers/fit0185.rs

omnitiles/drivers/
fit0185.rs
