omnitiles/control/
pid.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Generic PID controller for closed-loop control.
5//!
6//! Works in `no_std` and does not allocate memory.
7
8/// PID controller with tunable gains and output clamping.
9pub struct Pid {
10    /// Proportional gain
11    kp: f32,
12    /// Integral gain
13    ki: f32,
14    /// Derivative gain
15    kd: f32,
16
17    /// Integrator state
18    integral: f32,
19    /// Last process variable (for derivative term)
20    prev_measurement: f32,
21
22    /// Output clamp
23    out_min: f32,
24    out_max: f32,
25
26    /// Integral anti-windup clamp
27    int_min: f32,
28    int_max: f32,
29
30    first_update: bool,
31}
32
33impl Pid {
34    /// Create a new PID controller.
35    ///
36    /// `kp`, `ki`, `kd` are the gain constants.
37    pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
38        Self {
39            kp,
40            ki,
41            kd,
42
43            integral: 0.0,
44            prev_measurement: 0.0,
45
46            out_min: -1.0,
47            out_max: 1.0,
48
49            int_min: -1.0,
50            int_max: 1.0,
51
52            first_update: true,
53        }
54    }
55
56    /// Set output limits.
57    pub fn with_output_limits(mut self, min: f32, max: f32) -> Self {
58        self.out_min = min;
59        self.out_max = max;
60        self
61    }
62
63    /// Set integral limits for anti-windup.
64    pub fn with_integral_limits(mut self, min: f32, max: f32) -> Self {
65        self.int_min = min;
66        self.int_max = max;
67        self
68    }
69
70    /// Reset integrator + derivative history.
71    pub fn reset(&mut self) {
72        self.integral = 0.0;
73        self.prev_measurement = 0.0;
74        self.first_update = true;
75    }
76
77    /// Update the controller.
78    ///
79    /// `setpoint` — desired value  
80    /// `measurement` — current value  
81    /// `dt` — timestep in seconds (e.g. 0.02 for 50 Hz control loop)
82    ///
83    /// Returns a normalized command in [`out_min`, `out_max`] which can be mapped to motor drive.
84    pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 {
85        let error = setpoint - measurement;
86
87        // ----- P term -----
88        let p = self.kp * error;
89
90        // ----- I term -----
91        self.integral += error * dt * self.ki;
92
93        // Anti-windup clamp
94        if self.integral > self.int_max {
95            self.integral = self.int_max;
96        }
97        if self.integral < self.int_min {
98            self.integral = self.int_min;
99        }
100
101        let i = self.integral;
102
103        // ----- D term (on measurement to reduce noise sensitivity) -----
104        let d = if self.first_update {
105            self.first_update = false;
106            0.0
107        } else {
108            let dv = self.prev_measurement - measurement;
109            self.kd * (dv / dt)
110        };
111        self.prev_measurement = measurement;
112
113        // ----- Output clamp -----
114        let mut out = p + i + d;
115        if out > self.out_max {
116            out = self.out_max;
117        }
118        if out < self.out_min {
119            out = self.out_min;
120        }
121
122        out
123    }
124}
omnitiles/control/pid.rs

omnitiles/control/
pid.rs
