omnitiles/drivers/
actuonix_linear.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Generic driver for Actuonix 16-series (P16, T16) Linear Actuators with potentiometer control.
5//!
6//! Wraps a DRV8873 H-Bridge for motor control and an ADC channel for position feedback.
7//!
8//! Wiring (Option -P):
9//! - Pin 1 (Orange): Potentiometer Ground
10//! - Pin 2 (Purple): Potentiometer Wiper (ADC Input)
11//! - Pin 3 (Red):    Motor Terminal A (+)
12//! - Pin 4 (Black):  Motor Terminal B (-)
13//! - Pin 5 (Yellow): Potentiometer Reference (3.3V)
14
15use crate::drivers::drv8873::{Drv8873, Fault};
16use crate::hw::SpiBus;
17
18use stm32f7xx_hal::{
19    gpio::{self, Output, PushPull},
20    prelude::*,
21    spi,
22};
23
24/// Logical drive direction for the linear actuator.
25#[derive(Copy, Clone, Debug, PartialEq)]
26pub enum Direction {
27    Extend,
28    Retract,
29    Brake,
30}
31
32/// Generic driver for Actuonix linear actuators (P16, T16).
33///
34/// `ReadPos` is a closure that returns the raw 12-bit ADC reading (0..4095).
35pub struct ActuonixLinear<
36    const CS_P: char,
37    const CS_N: u8,
38    const SLP_P: char,
39    const SLP_N: u8,
40    const DIS_P: char,
41    const DIS_N: u8,
42    Pwm1,
43    Pwm2,
44    ReadPos,
45> {
46    drv: Drv8873<CS_P, CS_N>,
47    pwm1: Pwm1,
48    pwm2: Pwm2,
49    nsleep: gpio::Pin<SLP_P, SLP_N, Output<PushPull>>,
50    disable: gpio::Pin<DIS_P, DIS_N, Output<PushPull>>,
51    read_position: ReadPos,
52    adc_history: [u16; 5],
53    adc_idx: usize,
54    stroke_len_mm: f32,
55    buffer_bottom_mm: f32,
56    buffer_top_mm: f32,
57    current_speed: f32,
58    limit_brake_active: bool,
59}
60
61impl<
62        const CS_P: char,
63        const CS_N: u8,
64        const SLP_P: char,
65        const SLP_N: u8,
66        const DIS_P: char,
67        const DIS_N: u8,
68        Pwm1,
69        Pwm2,
70        ReadPos,
71    > ActuonixLinear<CS_P, CS_N, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos>
72where
73    Pwm1: _embedded_hal_PwmPin<Duty = u16>,
74    Pwm2: _embedded_hal_PwmPin<Duty = u16>,
75    ReadPos: FnMut() -> u16,
76{
77    /// Construct a new Actuonix driver with Hardware PWM.
78    pub fn new<SlpMode, DisMode>(
79        drv: Drv8873<CS_P, CS_N>,
80        pwm1: Pwm1,
81        pwm2: Pwm2,
82        nsleep: gpio::Pin<SLP_P, SLP_N, SlpMode>,
83        disable: gpio::Pin<DIS_P, DIS_N, DisMode>,
84        mut read_position: ReadPos,
85        stroke_len_mm: f32,
86        buffer_bottom_mm: f32,
87        buffer_top_mm: f32,
88    ) -> Self {
89        let mut nsleep = nsleep.into_push_pull_output();
90        let mut disable = disable.into_push_pull_output();
91
92        // Default: Awake, Enabled
93        nsleep.set_high();
94        disable.set_low();
95
96        let initial_pos = (read_position)();
97
98        Self {
99            drv,
100            pwm1,
101            pwm2,
102            nsleep,
103            disable,
104            read_position,
105            adc_history: [initial_pos; 5],
106            adc_idx: 0,
107            stroke_len_mm,
108            buffer_bottom_mm,
109            buffer_top_mm,
110            current_speed: 0.0,
111            limit_brake_active: false,
112        }
113    }
114
115    /// Set the motor speed and direction.
116    ///
117    /// `speed` - A float from -1.0 (Full Retract) to 1.0 (Full Extend).
118    pub fn set_speed(&mut self, speed: f32) {
119        // Any explicit speed command clears "limit brake" state.
120        self.limit_brake_active = false;
121
122        // Clamp speed to valid range
123        let mut speed = speed.clamp(-1.0, 1.0);
124
125        let pos = self.position_mm();
126        let max_pos = self.stroke_len_mm - self.buffer_top_mm;
127        let min_pos = self.buffer_bottom_mm;
128
129        // Prevent starting a movement that goes deeper into the out-of-bounds area
130        if speed > 0.0 && pos >= max_pos {
131            speed = 0.0;
132        } else if speed < 0.0 && pos <= min_pos {
133            speed = 0.0;
134        }
135
136        self.current_speed = speed;
137
138        let max_duty = self.pwm1.get_max_duty(); // Assuming Pwm1/Pwm2 have same resolution
139
140        // Calculate target duty cycle
141        let duty = (speed.abs() * max_duty as f32) as u16;
142
143        if speed > 0.001 {
144            // Extend: IN1 PWM, IN2 Low
145            self.pwm1.set_duty(duty);
146            self.pwm2.set_duty(0);
147            self.pwm1.enable();
148            self.pwm2.enable();
149        } else if speed < -0.001 {
150            // Retract: IN1 Low, IN2 PWM
151            self.pwm1.set_duty(0);
152            self.pwm2.set_duty(duty);
153            self.pwm1.enable();
154            self.pwm2.enable();
155        } else {
156            self.brake();
157        }
158    }
159
160    /// Continuously check the ADC position and brake if the actuator exceeds the software limits.
161    /// This should be called regularly in the main application loop.
162    pub fn enforce_limits(&mut self) {
163        // If we aren't moving, no need to check
164        if self.current_speed.abs() < 0.001 {
165            return;
166        }
167
168        let pos = self.position_mm();
169        let max_pos = self.stroke_len_mm - self.buffer_top_mm;
170        let min_pos = self.buffer_bottom_mm;
171
172        if self.current_speed > 0.0 && pos >= max_pos {
173            self.brake_due_to_limit();
174            self.current_speed = 0.0; // Reset state
175        } else if self.current_speed < 0.0 && pos <= min_pos {
176            self.brake_due_to_limit();
177            self.current_speed = 0.0; // Reset state
178        }
179    }
180
181    /// Extend the actuator at full speed.
182    #[inline]
183    pub fn extend(&mut self) {
184        self.set_speed(1.0);
185    }
186
187    /// Retract the actuator at full speed.
188    #[inline]
189    pub fn retract(&mut self) {
190        self.set_speed(-1.0);
191    }
192
193    /// Brake (stops quickly by shorting motor terminals).
194    #[inline]
195    pub fn brake(&mut self) {
196        self.limit_brake_active = false;
197        self.brake_raw();
198    }
199
200    #[inline]
201    fn brake_due_to_limit(&mut self) {
202        self.limit_brake_active = true;
203        self.brake_raw();
204    }
205
206    #[inline]
207    fn brake_raw(&mut self) {
208        let max = self.pwm1.get_max_duty();
209        self.pwm1.set_duty(max);
210        self.pwm2.set_duty(max);
211        self.pwm1.enable();
212        self.pwm2.enable();
213    }
214
215    /// True when we are currently braking due to software limit enforcement.
216    #[inline]
217    pub fn is_limit_braking(&self) -> bool {
218        self.limit_brake_active
219    }
220
221    /// Read raw 12-bit ADC value (0-4095) with hardware oversampling and a median filter.
222    #[inline]
223    pub fn position_raw(&mut self) -> u16 {
224        // 1. Get fresh hardware-oversampled reading
225        let raw = (self.read_position)();
226
227        // 2. Store in ring buffer
228        self.adc_history[self.adc_idx] = raw;
229        self.adc_idx = (self.adc_idx + 1) % 5;
230
231        // 3. Calculate and return Median
232        let mut sorted = self.adc_history;
233        sorted.sort_unstable();
234
235        sorted[2]
236    }
237
238    /// Read position as a fraction (0.0 = Retracted, 1.0 = Extended).
239    pub fn position_percent(&mut self) -> f32 {
240        let raw = self.position_raw();
241        // 12-bit ADC = 4095 max.
242        (raw as f32) / 4095.0
243    }
244
245    /// Read position in millimeters.
246    pub fn position_mm(&mut self) -> f32 {
247        self.position_percent() * self.stroke_len_mm
248    }
249
250    /// Get the max stroke length.
251    pub fn stroke_len_mm(&self) -> f32 {
252        self.stroke_len_mm
253    }
254
255    /// Access the inner DRV8873 for fault reading.
256    pub fn drv(&mut self) -> &mut Drv8873<CS_P, CS_N> {
257        &mut self.drv
258    }
259
260    /// Put the driver into sleep mode.
261    ///
262    /// This shuts down most of the internal circuitry to reduce power consumption.
263    #[inline]
264    pub fn sleep(&mut self) {
265        self.nsleep.set_low();
266    }
267
268    /// Wake the driver from sleep mode.
269    #[inline]
270    pub fn wake(&mut self) {
271        self.nsleep.set_high();
272    }
273
274    /// Enable the motor and wake the driver if in sleep.
275    #[inline]
276    pub fn enable_outputs(&mut self) {
277        self.wake();
278        self.disable.set_low();
279    }
280
281    /// Disable the motor and brake.
282    #[inline]
283    pub fn disable_outputs(&mut self) {
284        self.brake();
285        self.disable.set_high();
286    }
287
288    /// Read the FAULT status register from the DRV8873.
289    pub fn read_fault<I, PINS>(
290        &mut self,
291        spi_bus: &mut SpiBus<I, PINS>,
292    ) -> Result<Fault, spi::Error>
293    where
294        I: spi::Instance,
295        PINS: spi::Pins<I>,
296    {
297        self.drv.read_fault(spi_bus)
298    }
299}
omnitiles/drivers/actuonix_linear.rs

omnitiles/drivers/
actuonix_linear.rs
