omnitiles/control/
lift_controller.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Closed-loop controller for the linear lift actuator.
5//!
6//! This controller wraps a [`LiftMotor`] and provides a periodic `step()` function that computes a
7//! drive command using PID control and applies it to the motor.
8//!
9//! Typical usage pattern:
10//!
11//! ```no_run
12//! controller.set_target_height_mm(150.0);
13//!
14//! loop {
15//!     controller.step(dt_seconds);
16//!     delay.delay_ms(10_u32);
17//! }
18//! ```
19
20use crate::control::Pid;
21use crate::motors::LiftMotor;
22
23/// Operating mode of the lift controller.
24#[derive(Copy, Clone, Debug)]
25pub enum LiftMode {
26    /// Regular closed-loop control toward a height target.
27    PositionControl,
28
29    /// Motor is manually disabled (coast).
30    Disabled,
31}
32
33/// Controller state and configuration.
34pub struct LiftController<
35    const CS_P: char,
36    const CS_N: u8,
37    const IN1_P: char,
38    const IN1_N: u8,
39    const IN2_P: char,
40    const IN2_N: u8,
41    const SLP_P: char,
42    const SLP_N: u8,
43    const DIS_P: char,
44    const DIS_N: u8,
45> {
46    motor: LiftMotor<CS_P, CS_N, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N>,
47    pid: Pid,
48    mode: LiftMode,
49
50    /// Commanded height (mm)
51    target_height_mm: f32,
52
53    /// Safety limits (mm)
54    min_height_mm: f32,
55    max_height_mm: f32,
56
57    /// Deadband in mm, "close enough" to target
58    on_target_tolerance_mm: f32,
59}
60
61impl<
62        const CS_P: char,
63        const CS_N: u8,
64        const IN1_P: char,
65        const IN1_N: u8,
66        const IN2_P: char,
67        const IN2_N: u8,
68        const SLP_P: char,
69        const SLP_N: u8,
70        const DIS_P: char,
71        const DIS_N: u8,
72    > LiftController<CS_P, CS_N, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N>
73{
74    /// Create a new lift controller with PID gains and limits.
75    pub fn new(
76        motor: LiftMotor<CS_P, CS_N, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N>,
77        pid: Pid,
78        min_height_mm: f32,
79        max_height_mm: f32,
80        on_target_tolerance_mm: f32,
81    ) -> Self {
82        Self {
83            motor,
84            pid,
85            mode: LiftMode::PositionControl,
86            target_height_mm: 0.0,
87            min_height_mm,
88            max_height_mm,
89            on_target_tolerance_mm,
90        }
91    }
92
93    /// Set a new target height (mm), automatically clamped to limits.
94    pub fn set_target_height_mm(&mut self, mm: f32) {
95        let clamped = mm.clamp(self.min_height_mm, self.max_height_mm);
96        self.target_height_mm = clamped;
97        self.pid.reset();
98    }
99
100    /// Directly disable the lift (coast).
101    pub fn disable(&mut self) {
102        self.mode = LiftMode::Disabled;
103        self.motor.inner_motor().coast();
104    }
105
106    /// Re-enable position control.
107    pub fn enable(&mut self) {
108        self.mode = LiftMode::PositionControl;
109    }
110
111    /// Current measured height (mm).
112    #[inline]
113    pub fn height_mm(&self) -> f32 {
114        self.motor.height_mm()
115    }
116
117    /// Returns true if the lift is within tolerance of its commanded height.
118    pub fn on_target(&self) -> bool {
119        let err = (self.height_mm() - self.target_height_mm).abs();
120        err <= self.on_target_tolerance_mm
121    }
122
123    /// Run one control step.
124    ///
125    /// `dt` — time delta in seconds since the previous call, e.g. 0.01 for 10ms.
126    pub fn step(&mut self, dt: f32) {
127        match self.mode {
128            LiftMode::Disabled => {
129                self.motor.inner_motor().coast();
130                return;
131            }
132
133            LiftMode::PositionControl => {
134                let current = self.motor.height_mm();
135                let target = self
136                    .target_height_mm
137                    .clamp(self.min_height_mm, self.max_height_mm);
138                let error = target - current;
139
140                // PID output
141                let mut u = self.pid.update(target, current, dt);
142
143                // Clamp to [-1.0, 1.0]
144                if u > 1.0 {
145                    u = 1.0;
146                }
147                if u < -1.0 {
148                    u = -1.0;
149                }
150
151                // Near-target deadband
152                if error.abs() <= self.on_target_tolerance_mm {
153                    self.motor.inner_motor().brake();
154                    return;
155                }
156
157                // Apply PID output
158                self.motor.apply_pid_output(u);
159            }
160        }
161    }
162
163    /// Expose the underlying LiftMotor.
164    pub fn motor(
165        &self,
166    ) -> &LiftMotor<CS_P, CS_N, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N> {
167        &self.motor
168    }
169
170    /// Mutable access to the underlying LiftMotor.
171    pub fn motor_mut(
172        &mut self,
173    ) -> &mut LiftMotor<CS_P, CS_N, IN1_P, IN1_N, IN2_P, IN2_N, SLP_P, SLP_N, DIS_P, DIS_N> {
174        &mut self.motor
175    }
176}
omnitiles/control/lift_controller.rs

omnitiles/control/
lift_controller.rs
