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

omnitiles/drivers/
fit0185.rs
