omnitiles/control/
linear_controller.rs

1// SPDX-License-Identifier: MIT
2// © 2025-2026 Christopher Liu
3
4//! PID position control for Actuonix linear actuators.
5
6use crate::control::Pid;
7use crate::drivers::ActuonixLinear;
8use crate::hw::spi::CsControl;
9use stm32f7xx_hal::prelude::*;
10
11#[derive(Copy, Clone, Debug, PartialEq)]
12pub enum LinearMode {
13    PositionControl,
14    Disabled,
15}
16
17#[derive(Copy, Clone, Debug, PartialEq, Eq)]
18pub enum ControlError {
19    /// PID was requested but no pot channels are enabled on the actuator.
20    NoPositionFeedback,
21}
22
23/// PID position controller for an Actuonix linear actuator. Call [`step`](Self::step) periodically.
24pub struct LinearController<
25    CS: CsControl,
26    const SLP_P: char,
27    const SLP_N: u8,
28    const DIS_P: char,
29    const DIS_N: u8,
30    Pwm1,
31    Pwm2,
32    ReadPos,
33    const N: usize,
34> {
35    pub actuator: ActuonixLinear<CS, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos, N>,
36    pub pid: Pid,
37    pub mode: LinearMode,
38
39    pub target_position_mm: f32,
40    pub min_position_mm: f32,
41    pub max_position_mm: f32,
42    pub on_target_tolerance_mm: f32,
43}
44
45impl<
46        CS: CsControl,
47        const SLP_P: char,
48        const SLP_N: u8,
49        const DIS_P: char,
50        const DIS_N: u8,
51        Pwm1,
52        Pwm2,
53        ReadPos,
54        const N: usize,
55    > LinearController<CS, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos, N>
56where
57    Pwm1: _embedded_hal_PwmPin<Duty = u16>,
58    Pwm2: _embedded_hal_PwmPin<Duty = u16>,
59    ReadPos: FnMut() -> [u16; N],
60{
61    /// Create a new linear controller with PID gains and limits.
62    pub fn new(
63        actuator: ActuonixLinear<CS, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos, N>,
64        pid: Pid,
65        min_position_mm: f32,
66        max_position_mm: f32,
67        on_target_tolerance_mm: f32,
68    ) -> Self {
69        Self {
70            actuator,
71            pid,
72            mode: LinearMode::PositionControl,
73            target_position_mm: 0.0,
74            min_position_mm,
75            max_position_mm,
76            on_target_tolerance_mm,
77        }
78    }
79
80    /// Set a new target position (mm), automatically clamped to limits.
81    pub fn set_target_position_mm(&mut self, mm: f32) {
82        self.target_position_mm = mm.clamp(self.min_position_mm, self.max_position_mm);
83        self.pid.reset();
84    }
85
86    /// Run one control step. Returns `Err(NoPositionFeedback)` if the mode is
87    /// `PositionControl` but the actuator has no enabled pot channels; in that
88    /// case the actuator is braked for safety.
89    pub fn step(&mut self, dt: f32) -> Result<(), ControlError> {
90        self.actuator.enforce_limits();
91
92        match self.mode {
93            LinearMode::Disabled => Ok(()),
94
95            LinearMode::PositionControl => {
96                let Some(position_mm) = self.actuator.position_mm() else {
97                    self.actuator.brake();
98                    return Err(ControlError::NoPositionFeedback);
99                };
100                let target = self
101                    .target_position_mm
102                    .clamp(self.min_position_mm, self.max_position_mm);
103                let error = target - position_mm;
104
105                if error.abs() <= self.on_target_tolerance_mm {
106                    self.actuator.brake();
107                    return Ok(());
108                }
109
110                let output = self.pid.update(target, position_mm, dt);
111                self.actuator.set_speed(output);
112                Ok(())
113            }
114        }
115    }
116}
omnitiles/control/linear_controller.rs

omnitiles/control/
linear_controller.rs
