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 stm32f7xx_hal::prelude::*;
9
10#[derive(Copy, Clone, Debug, PartialEq)]
11pub enum LinearMode {
12    PositionControl,
13    Disabled,
14}
15
16/// PID position controller for an Actuonix linear actuator. Call [`step`](Self::step) periodically.
17pub struct LinearController<
18    const CS_P: char,
19    const CS_N: u8,
20    const SLP_P: char,
21    const SLP_N: u8,
22    const DIS_P: char,
23    const DIS_N: u8,
24    Pwm1,
25    Pwm2,
26    ReadPos,
27> {
28    pub actuator: ActuonixLinear<CS_P, CS_N, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos>,
29    pub pid: Pid,
30    pub mode: LinearMode,
31
32    pub target_position_mm: f32,
33    pub min_position_mm: f32,
34    pub max_position_mm: f32,
35    pub on_target_tolerance_mm: f32,
36}
37
38impl<
39        const CS_P: char,
40        const CS_N: u8,
41        const SLP_P: char,
42        const SLP_N: u8,
43        const DIS_P: char,
44        const DIS_N: u8,
45        Pwm1,
46        Pwm2,
47        ReadPos,
48    > LinearController<CS_P, CS_N, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos>
49where
50    Pwm1: _embedded_hal_PwmPin<Duty = u16>,
51    Pwm2: _embedded_hal_PwmPin<Duty = u16>,
52    ReadPos: FnMut() -> u16,
53{
54    /// Create a new linear controller with PID gains and limits.
55    pub fn new(
56        actuator: ActuonixLinear<CS_P, CS_N, SLP_P, SLP_N, DIS_P, DIS_N, Pwm1, Pwm2, ReadPos>,
57        pid: Pid,
58        min_position_mm: f32,
59        max_position_mm: f32,
60        on_target_tolerance_mm: f32,
61    ) -> Self {
62        Self {
63            actuator,
64            pid,
65            mode: LinearMode::PositionControl,
66            target_position_mm: 0.0,
67            min_position_mm,
68            max_position_mm,
69            on_target_tolerance_mm,
70        }
71    }
72
73    /// Set a new target position (mm), automatically clamped to limits.
74    pub fn set_target_position_mm(&mut self, mm: f32) {
75        self.target_position_mm = mm.clamp(self.min_position_mm, self.max_position_mm);
76        self.pid.reset();
77    }
78
79    /// Run one control step.
80    pub fn step(&mut self, dt: f32) {
81        self.actuator.enforce_limits();
82
83        match self.mode {
84            LinearMode::Disabled => return,
85
86            LinearMode::PositionControl => {
87                let position_mm = self.actuator.position_mm();
88                let target = self
89                    .target_position_mm
90                    .clamp(self.min_position_mm, self.max_position_mm);
91                let error = target - position_mm;
92
93                if error.abs() <= self.on_target_tolerance_mm {
94                    self.actuator.brake();
95                    return;
96                }
97
98                let output = self.pid.update(target, position_mm, dt);
99                self.actuator.set_speed(output);
100            }
101        }
102    }
103}
omnitiles/control/linear_controller.rs

omnitiles/control/
linear_controller.rs
