advanced_pid/
vel_pid.rs

1//! The `vel_pid` module provides a velocity form PID controller.
2//!
3//! `VelPid` 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//! ```rust
8//! use advanced_pid::{prelude::*, PidConfig, VelPid};
9//!
10//! let config = PidConfig::new(1.0, 0.3, 0.1).with_limits(-1.0, 1.0);
11//! let mut pid = VelPid::new(config);
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/// `VelPid` is a structure that implements the [`PidController`] trait.
24#[derive(Debug, Clone)]
25pub struct VelPid {
26    config: PidConfig,
27    output: FloatType,
28    pre_error: FloatType,
29    pre_p_term: FloatType,
30    d_term_lpf: FloatType,
31}
32
33impl Default for VelPid {
34    /// Creates a new `VelPid` with the default configuration.
35    fn default() -> Self {
36        Self::new(PidConfig::default())
37    }
38}
39
40impl PidController for VelPid {
41    /// Creates a new `VelPid` with the specified configuration.
42    fn new(config: PidConfig) -> Self {
43        Self {
44            config,
45            output: 0.0,
46            pre_error: 0.0,
47            pre_p_term: FloatType::NAN,
48            d_term_lpf: 0.0,
49        }
50    }
51
52    /// Updates the `VelPid` controller with the specified set point, actual value, and time delta.
53    /// Returns the controller output.
54    fn update(&mut self, set_point: FloatType, actual: FloatType, dt: FloatType) -> FloatType {
55        debug_assert!(dt > 0.0, "dt must be positive");
56        let error = set_point - actual;
57        let p_term = (error - self.pre_error) / dt;
58        let d_term = if self.pre_p_term.is_nan() {
59            0.0
60        } else {
61            (p_term - self.pre_p_term) / dt
62        };
63        self.d_term_lpf += (d_term - self.d_term_lpf) / 8.0;
64        let du = self.config.gain.kp * p_term
65            + self.config.gain.ki * error
66            + self.config.gain.kd * self.d_term_lpf;
67        self.pre_error = error;
68        self.pre_p_term = p_term;
69        self.output = (self.output + du * dt).clamp(self.config.min, self.config.max);
70        self.output
71    }
72}
73
74#[cfg(test)]
75mod tests {
76    use super::*;
77
78    #[test]
79    fn test_vel_pid_p() {
80        let gain = crate::PidGain {
81            kp: 1.0,
82            ki: 0.0,
83            kd: 0.0,
84        };
85        let mut pid = VelPid::new(gain.into());
86
87        let output = pid.update(1.0, 0.0, 1.0);
88        assert_eq!(output, 1.0);
89    }
90
91    #[test]
92    fn test_vel_pid_i() {
93        let gain = crate::PidGain {
94            kp: 0.0,
95            ki: 1.0,
96            kd: 0.0,
97        };
98        let mut pid = VelPid::new(gain.into());
99
100        let output = pid.update(1.0, 0.0, 1.0);
101        assert_eq!(output, 1.0);
102        let output = pid.update(1.0, 0.0, 1.0);
103        assert_eq!(output, 2.0);
104        let output = pid.update(1.0, 0.0, 1.0);
105        assert_eq!(output, 3.0);
106    }
107
108    #[test]
109    fn test_vel_pid_d() {
110        let gain = crate::PidGain {
111            kp: 0.0,
112            ki: 0.0,
113            kd: 1.0,
114        };
115        let mut pid = VelPid::new(gain.into());
116
117        let output = pid.update(1.0, 0.0, 1.0);
118        assert_eq!(output, 0.0);
119        let output = pid.update(1.0, 5.0, 1.0);
120        assert!(output < 0.0, "d_term: {} must be lesser than 0.0", output);
121    }
122
123    #[test]
124    fn test_vel_pid_limits() {
125        let config = PidConfig::new(1.0, 0.0, 0.0).with_limits(-0.5, 0.5);
126        let mut pid = VelPid::new(config);
127
128        let output = pid.update(1.0, 0.0, 1.0);
129        assert_eq!(output, 0.5);
130        let output = pid.update(-1.0, 0.0, 1.0);
131        assert_eq!(output, -0.5);
132    }
133}
advanced_pid/vel_pid.rs

advanced_pid/
vel_pid.rs
