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

//! The `pi_d` module provides a PID controller where the derivative action is based on the process variable (PV).
//!
//! `PiD` 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::*, PiD, PidGain};
//!
//! let config = PidConfig::new(1.0, 0.1, 0.1).with_limits(-1.0, 1.0);
//! let mut pid = PiD::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;

/// `PiD` is a structure that implements the [`PidController`] trait.
#[derive(Debug, Clone)]
pub struct PiD {
    config: PidConfig,
    i_term: FloatType,
    pre_actual: FloatType,
}

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

impl PidController for PiD {
    /// Creates a new `PiD` with the specified configuration.
    fn new(config: PidConfig) -> Self {
        Self {
            config,
            i_term: 0.0,
            pre_actual: FloatType::NAN,
        }
    }

    /// Updates the `PiD` 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 {
        let error = set_point - actual;
        self.i_term += error * dt;
        let d_term = if self.pre_actual.is_nan() {
            0.0
        } else {
            (actual - self.pre_actual) / dt
        };
        let output = self.config.gain.kp * error + self.config.gain.ki * self.i_term
            - self.config.gain.kd * d_term;
        self.pre_actual = actual;
        output.clamp(self.config.min, self.config.max)
    }
}

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

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

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

    #[test]
    fn test_pi_d_controller_i() {
        let gain = crate::PidGain {
            kp: 0.0,
            ki: 1.0,
            kd: 0.0,
        };
        let mut pid = PiD::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_pi_d_controller_d() {
        let gain = crate::PidGain {
            kp: 0.0,
            ki: 0.0,
            kd: 1.0,
        };
        let mut pid = PiD::new(gain.into());

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