advanced_pid/
pid.rs

1//! The `pid` module provides a standard (position form) PID controller.
2//!
3//! `Pid` is a structure that implements the [`PidController`] trait, which provides methods for creating a new controller and updating the controller.
4//!
5//! # Examples
6//!
7//! ```
8//! use advanced_pid::{prelude::*, Pid, PidGain};
9//!
10//! let gain = PidGain { kp: 1.0, ki: 0.3, kd: 0.1 };
11//! let mut pid = Pid::new(gain.into());
12//!
13//! let target = 1.0;
14//! let actual = 0.0;
15//! let dt = 1.0;
16//!
17//! println!("{}", pid.update(target, actual, dt));
18//! ```
19use super::FloatType;
20use super::PidConfig;
21use super::PidController;
22
23/// `Pid` is a structure that implements the [`PidController`] trait.
24#[derive(Debug, Clone)]
25pub struct Pid {
26    config: PidConfig,
27    i_term: FloatType,
28    pre_error: FloatType,
29}
30
31impl Default for Pid {
32    /// Creates a new `Pid` with the default configuration.
33    fn default() -> Self {
34        Self::new(PidConfig::default())
35    }
36}
37
38impl PidController for Pid {
39    /// Creates a new `Pid` with the specified configuration.
40    fn new(config: PidConfig) -> Self {
41        Self {
42            config,
43            i_term: 0.0,
44            pre_error: FloatType::NAN,
45        }
46    }
47
48    /// Updates the `Pid` controller with the specified set point, actual value, and time delta.
49    /// Returns the controller output.
50    fn update(&mut self, set_point: FloatType, actual: FloatType, dt: FloatType) -> FloatType {
51        let error = set_point - actual;
52        self.i_term += error * dt;
53        let d_term = if self.pre_error.is_nan() {
54            0.0
55        } else {
56            (error - self.pre_error) / dt
57        };
58        let output = self.config.gain.kp * error
59            + self.config.gain.ki * self.i_term
60            + self.config.gain.kd * d_term;
61        self.pre_error = error;
62        output.clamp(self.config.min, self.config.max)
63    }
64}
65
66#[cfg(test)]
67mod tests {
68    use super::*;
69
70    #[test]
71    fn test_pid_controller_p() {
72        let gain = crate::PidGain {
73            kp: 1.0,
74            ki: 0.0,
75            kd: 0.0,
76        };
77        let mut pid = Pid::new(gain.into());
78
79        let output = pid.update(1.0, 0.0, 1.0);
80        assert_eq!(output, 1.0);
81    }
82
83    #[test]
84    fn test_pid_controller_i() {
85        let gain = crate::PidGain {
86            kp: 0.0,
87            ki: 1.0,
88            kd: 0.0,
89        };
90        let mut pid = Pid::new(gain.into());
91
92        let output = pid.update(1.0, 0.0, 1.0);
93        assert_eq!(output, 1.0);
94        let output = pid.update(1.0, 0.0, 1.0);
95        assert_eq!(output, 2.0);
96        let output = pid.update(1.0, 0.0, 1.0);
97        assert_eq!(output, 3.0);
98    }
99
100    #[test]
101    fn test_pid_controller_d() {
102        let gain = crate::PidGain {
103            kp: 0.0,
104            ki: 0.0,
105            kd: 1.0,
106        };
107        let mut pid = Pid::new(gain.into());
108
109        let output = pid.update(1.0, 0.0, 1.0);
110        assert_eq!(output, 0.0);
111        let output = pid.update(1.0, 1.0, 1.0);
112        assert_eq!(output, -1.0);
113    }
114
115    #[test]
116    fn test_pid_controller_limits() {
117        let config = PidConfig::new(1.0, 0.0, 0.0).with_limits(-0.5, 0.5);
118        let mut pid = Pid::new(config);
119
120        let output = pid.update(1.0, 0.0, 1.0);
121        assert_eq!(output, 0.5);
122        let output = pid.update(-1.0, 0.0, 1.0);
123        assert_eq!(output, -0.5);
124    }
125}
advanced_pid/pid.rs

advanced_pid/
pid.rs
