pid_ctrl/
lib.rs

1//! A proportional-integral-derivative (PID) controller.
2#![no_std]
3use num_traits::{float::FloatCore};
4#[cfg(feature = "serde")]
5use serde::{Deserialize, Serialize};
6
7// #[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, Debug, Display, Default)]
8#[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, Debug)]
9pub enum PidError {
10    LimitOutBound,
11}
12
13#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug)]
14#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
15pub struct Limits<T: FloatCore + core::default::Default> {
16    lower: T,
17    upper: T,
18}
19
20impl<T: FloatCore + core::default::Default> Limits<T> {
21    fn new() -> Self {
22        Limits{lower: T::neg_infinity(), upper: T::infinity()}
23    }
24
25    fn clamp(&self, val: T) -> T {
26        val.min(self.upper).max(self.lower)
27    }
28
29    pub fn set_limit(&mut self, val: T) -> &mut Self {
30        self.lower = -val.abs();
31        self.upper = val.abs();
32        self
33    }
34
35    pub fn try_set_upper(&mut self, val: T) -> Result<&mut Self, PidError> {
36        if self.lower <= val {
37            self.upper = val;
38            Ok(self)
39        }
40        else {
41            Err(PidError::LimitOutBound)
42        }
43    }
44
45    pub fn try_set_lower(&mut self, val: T) -> Result<&mut Self, PidError> {
46        if self.upper >= val {
47            self.lower = val;
48            Ok(self)
49        }
50        else {
51            Err(PidError::LimitOutBound)
52        }
53    }
54}
55
56impl<T: FloatCore + core::default::Default> Default for Limits<T> {
57    fn default() -> Self {
58        Self::new()
59    }
60}
61
62#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
63#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
64pub struct KPTerm<T: FloatCore + core::default::Default> {
65    pub limits: Limits<T>,
66    scale: T,
67}
68
69impl<T:FloatCore + core::default::Default> KPTerm<T> {
70    pub fn new() -> Self {
71        KPTerm::default()
72    }
73    pub fn set_scale(&mut self, val: T) -> &mut Self {
74        self.scale = val;
75        self
76    }
77    pub fn step(&self, offset: T) -> T {
78        self.limits.clamp(self.scale * offset)
79    }
80}
81
82#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
83#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
84pub struct KITerm<T: FloatCore + core::default::Default> {
85    pub limits: Limits<T>,
86    scale: T,
87    pub accumulate: T
88}
89
90impl<T:FloatCore + core::default::Default> KITerm<T> {
91    pub fn new() -> Self {
92        KITerm::default()
93    }
94    pub fn set_scale(&mut self, val: T) -> &mut Self {
95        self.scale = val;
96        self
97    }
98    pub fn step(&mut self, offset: T, tdelta: T) -> T {
99        let i = self.limits.clamp(self.scale * offset * tdelta + self.accumulate);
100        self.accumulate = i;
101        i
102    }
103}
104
105#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
106#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
107pub struct KDTerm<T: FloatCore + core::default::Default> {
108    pub limits: Limits<T>,
109    scale: T,
110    pub prev_measurement: T
111}
112
113impl<T:FloatCore + core::default::Default> KDTerm<T> {
114    pub fn new() -> Self {
115        KDTerm::default()
116    }
117    pub fn set_scale(&mut self, val: T) -> &mut Self {
118        self.scale = val;
119        self
120    }
121    pub fn step(&mut self, measurement: T, tdelta: T) -> T {
122        let d = self.limits.clamp(self.scale * (self.prev_measurement - measurement) / tdelta);
123        self.prev_measurement = measurement;
124        d
125    }
126}
127
128#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
129#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
130pub struct PidCtrl <T: FloatCore + core::default::Default> {
131    pub kp: KPTerm<T>,
132    pub ki: KITerm<T>,
133    pub kd: KDTerm<T>,
134    pub limits: Limits<T>,
135    
136    pub setpoint: T,
137}
138
139impl<T: FloatCore + core::default::Default> PidCtrl<T>
140    {
141        pub fn new() -> Self {
142            PidCtrl::default()
143        }
144
145        pub fn new_with_pid(p: T, i: T, d: T) -> Self {
146            Self{
147                kp: KPTerm{limits:Limits::new(), scale: p}, 
148                ki: KITerm{limits:Limits::new(), scale: i, accumulate:T::zero()}, 
149                kd: KDTerm{limits:Limits::new(), scale: d, prev_measurement:T::zero()}, 
150                limits: Limits::new(), setpoint: T::zero(),
151            }
152        }
153
154        pub fn init(&mut self, setpoint: T, prev_measurement: T) -> &mut Self {
155            self.setpoint = setpoint;
156            self.kd.prev_measurement = prev_measurement;
157            self
158        }
159
160        pub fn step(&mut self, input: PidIn<T>) -> PidOut<T> {
161            let offset = self.setpoint - input.measurement;
162            let p = self.kp.step(offset);
163            let i = self.ki.step(offset, input.tdelta);
164            let d = self.kd.step(input.measurement, input.tdelta);
165            PidOut::new(p, i, d, self.limits.clamp(p + i + d))
166        }
167    }
168
169#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
170#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
171pub struct PidIn <T: FloatCore + core::default::Default> {
172    measurement: T,
173    tdelta: T,
174}
175
176impl<T: FloatCore + core::default::Default> PidIn<T> {
177        pub fn new(measurement:T, tdelta:T) -> Self {
178            let tdelta_clamped = tdelta.min(T::infinity()).max(T::epsilon());
179            PidIn{measurement, tdelta: tdelta_clamped}
180        }
181    }
182
183#[derive(Copy, Clone, PartialEq, PartialOrd, Hash, Debug, Default)]
184#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
185pub struct PidOut <T: FloatCore + core::default::Default> {
186    pub p: T,
187    pub i: T,
188    pub d: T,
189    pub out: T,
190}
191
192impl<T: FloatCore + core::default::Default> PidOut<T> {
193        pub fn new(p:T, i:T, d:T, out:T) -> Self {
194            Self{p, i, d, out}
195        }
196    }
197
198#[cfg(test)]
199mod tests {
200    #[test]
201    fn limits_error() {
202        let mut pid = super::PidCtrl::new_with_pid(3.0, 2.0, 1.0);
203        pid.kp.limits.try_set_lower(10.0).unwrap();
204        assert_eq!(super::PidError::LimitOutBound, pid.kp.limits.try_set_upper(5.0).unwrap_err());
205    }
206
207    #[test]
208    fn kp() {
209        let kp = 0.2;
210        let measurement = 0.0;
211        let setpoint = 1.0;
212
213        let mut pid = super::PidCtrl::default();
214        pid.init(setpoint, 0.0);
215        pid.kp.set_scale(kp);
216
217        let kpterm = kp * (setpoint - measurement);
218
219        let inp = super::PidIn::new(measurement, 1.0);
220        assert_eq!(pid.step(inp), super::PidOut::new(kpterm, 0.0, 0.0, kpterm));
221    }
222
223    #[test]
224    fn ki() {
225        let ki = 1.0;
226        let measurement = 0.0;
227        let setpoint = 1.0;
228        let td = 1.0;
229
230        let mut pid = super::PidCtrl::default();
231        pid.init(setpoint, 0.0);
232        pid.ki.set_scale(ki);
233
234        let mut kiterm = 0.0;
235
236        kiterm += ki * (setpoint - measurement) * td;
237        let inp = super::PidIn::new(measurement, td);
238        assert_eq!(pid.step(inp), super::PidOut::new(0.0, kiterm, 0.0, kiterm));
239
240        kiterm += ki * (setpoint - measurement) * td;
241        let inp = super::PidIn::new(measurement, td);
242        assert_eq!(pid.step(inp), super::PidOut::new(0.0, kiterm, 0.0, kiterm));
243    }
244
245    #[test]
246    fn kd() {
247        let kd = 1.0;
248        let measurement = 0.0;
249        let setpoint = 1.0;
250        let td = 1.0;
251        
252        let mut prev = 0.0;
253
254        let mut pid = super::PidCtrl::default();
255        pid.init(setpoint, prev);
256        pid.kd.set_scale(kd);
257
258        let mut kdterm = kd * (measurement - prev) / td;
259        prev = measurement;
260        let inp = super::PidIn::new(measurement, td);
261        assert_eq!(pid.step(inp), super::PidOut::new(0.0, 0.0, kdterm, kdterm));
262
263        kdterm = kd * (measurement - prev) / td;
264        let inp = super::PidIn::new(measurement, td);
265        assert_eq!(pid.step(inp), super::PidOut::new(0.0, 0.0, kdterm, kdterm));
266    }
267}
pid_ctrl/lib.rs

pid_ctrl/
lib.rs
