control/
pid.rs

1use num_traits::float::FloatCore;
2
3/// Implementation of a proportional–integral–derivative controller.
4pub struct PID<T> {
5    /// Desired setpoint.
6    pub setpoint: T,
7
8    /// Error value from the previous update.
9    error: T,
10    /// Integral value from the previous update.
11    integral: T,
12    /// Derivative value from the previous update.
13    derivative: T,
14    /// Previous measurement value.
15    measurement: T,
16
17    /// Proportional coefficient.
18    p: T,
19    /// Integral coefficient.
20    i: T,
21    /// Derivative coefficient.
22    d: T,
23    /// Coefficient for the derivative low-pass filter.
24    t: T,
25
26    /// Lower bound of the integral term.
27    imin: T,
28    /// Upper bound of the integral term.
29    imax: T,
30
31    /// Lower bound of the controller output.
32    omin: T,
33    /// Upper bound of the controller output.
34    omax: T,
35}
36
37impl<T: FloatCore> PID<T> {
38    /// Creates a new `PID` with a proportional gain of `kp`, integral gain of `ki`
39    /// and derivative gain of `kd`.
40    ///
41    /// `tau` represents the time constant of the derivative low-pass filter.
42    ///
43    /// `sampling_time` is the time difference in seconds between two consecutive
44    /// step operations.
45    ///
46    /// # Examples
47    ///
48    /// ```
49    ///
50    /// ```
51    pub fn new(kp: T, ki: T, kd: T, tau: T, sampling_time: T, setpoint: T) -> Self {
52        let two = T::from(2.0_f32).expect("Unable to cast from 2.0");
53        let half = T::from(0.5_f32).expect("Unable to cast from 0.5");
54
55        Self {
56            setpoint,
57
58            error: T::zero(),
59            integral: T::zero(),
60            derivative: T::zero(),
61            measurement: T::zero(),
62
63            p: kp,
64            i: half * ki * sampling_time,
65            d: -two * kd,
66            t: (two * tau - sampling_time) / (two * tau + sampling_time),
67
68            imin: T::neg_infinity(),
69            imax: T::infinity(),
70
71            omin: T::neg_infinity(),
72            omax: T::infinity(),
73        }
74    }
75
76    /// Indicates that the integral term should be restricted to a certain interval.
77    /// Useful to prevent [integral windup].
78    ///
79    /// [integral windup]: https://en.wikipedia.org/wiki/PID_controller#Integral_windup
80    ///
81    /// # Panics
82    ///
83    /// Panics if `min` > `max`.
84    ///
85    /// # Examples
86    ///
87    /// ```
88    ///
89    /// ```
90    pub fn bound_integral(&mut self, min: T, max: T) -> &mut Self {
91        assert!(min <= max);
92        self.imin = min;
93        self.imax = max;
94        self
95    }
96
97    /// Indicates that the controller output should be restricted to a certain interval.
98    ///
99    /// # Panics
100    ///
101    /// Panics if `min` > `max`.
102    ///
103    /// # Examples
104    ///
105    /// ```
106    ///
107    /// ```
108    pub fn bound_output(&mut self, min: T, max: T) -> &mut Self {
109        assert!(min <= max);
110        self.omin = min;
111        self.omax = max;
112        self
113    }
114
115    /// Performs a single step of the control loop. It should be called exactly
116    /// once every `sampling_time` seconds.
117    pub fn step(&mut self, measurement: T) -> T {
118        let error = self.setpoint - measurement;
119
120        let proportional = self.p * error;
121        // Calculate integral term and clamp it to prevent windup.
122        let integral = self.i * (error + self.error) + self.integral;
123        self.integral = num_traits::clamp(integral, self.imin, self.imax);
124        // Derivative on measurement to prevent a kick during setpoint changes.
125        self.derivative = self.d * (measurement - self.measurement) + self.t * self.derivative;
126
127        let output = proportional + self.integral + self.derivative;
128        num_traits::clamp(output, self.omin, self.omax)
129    }
130}
control/pid.rs

control/
pid.rs
