1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

//! The `vel_pid` module provides a velocity form PID controller.
//!
//! `VelPid` is a structure that implements the [`PidController`] trait, which provides methods for creating a new controller and updating the controller.
//!
//! # Examples
//!
//! ```rust
//! use advanced_pid::{prelude::*, PidConfig, VelPid};
//!
//! let config = PidConfig::new(1.0, 0.3, 0.1).with_limits(-1.0, 1.0);
//! let mut pid = VelPid::new(config);
//!
//! let target = 1.0;
//! let actual = 0.0;
//! let dt = 1.0;
//!
//! println!("{}", pid.update(target, actual, dt));
//! ```
use super::FloatType;
use super::PidConfig;
use super::PidController;

/// `VelPid` is a structure that implements the [`PidController`] trait.
#[derive(Debug, Clone)]
pub struct VelPid {
    config: PidConfig,
    output: FloatType,
    pre_error: FloatType,
    pre_p_term: FloatType,
    d_term_lpf: FloatType,
}

impl Default for VelPid {
    /// Creates a new `VelPid` with the default configuration.
    fn default() -> Self {
        Self::new(PidConfig::default())
    }
}

impl PidController for VelPid {
    /// Creates a new `VelPid` with the specified configuration.
    fn new(config: PidConfig) -> Self {
        Self {
            config,
            output: 0.0,
            pre_error: 0.0,
            pre_p_term: FloatType::NAN,
            d_term_lpf: 0.0,
        }
    }

    /// Updates the `VelPid` controller with the specified set point, actual value, and time delta.
    /// Returns the controller output.
    fn update(&mut self, set_point: FloatType, actual: FloatType, dt: FloatType) -> FloatType {
        debug_assert!(dt > 0.0, "dt must be positive");
        let error = set_point - actual;
        let p_term = (error - self.pre_error) / dt;
        let d_term = if self.pre_p_term.is_nan() {
            0.0
        } else {
            (p_term - self.pre_p_term) / dt
        };
        self.d_term_lpf += (d_term - self.d_term_lpf) / 8.0;
        let du = self.config.gain.kp * p_term
            + self.config.gain.ki * error
            + self.config.gain.kd * self.d_term_lpf;
        self.pre_error = error;
        self.pre_p_term = p_term;
        self.output = (self.output + du).clamp(self.config.min, self.config.max);
        self.output
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_vel_pid_p() {
        let gain = crate::PidGain {
            kp: 1.0,
            ki: 0.0,
            kd: 0.0,
        };
        let mut pid = VelPid::new(gain.into());

        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 1.0);
    }

    #[test]
    fn test_vel_pid_i() {
        let gain = crate::PidGain {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = VelPid::new(gain.into());

        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 1.0);
        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 2.0);
        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 3.0);
    }

    #[test]
    fn test_vel_pid_d() {
        let gain = crate::PidGain {
            kp: 0.0,
            ki: 0.0,
            kd: 1.0,
        };
        let mut pid = VelPid::new(gain.into());

        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 0.0);
        let output = pid.update(1.0, 5.0, 1.0);
        assert!(output < 0.0, "d_term: {} must be lesser than 0.0", output);
    }

    #[test]
    fn test_vel_pid_limits() {
        let config = PidConfig::new(1.0, 0.0, 0.0).with_limits(-0.5, 0.5);
        let mut pid = VelPid::new(config);

        let output = pid.update(1.0, 0.0, 1.0);
        assert_eq!(output, 0.5);
        let output = pid.update(-1.0, 0.0, 1.0);
        assert_eq!(output, -0.5);
    }
}