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

use super::FloatType;
use super::PidConfig;
use super::PidController;

#[derive(Debug, Clone)]
pub struct Ipd {
    config: PidConfig,
    i_term: FloatType,
    pre_actual: FloatType,
}

impl Default for Ipd {
    fn default() -> Self {
        Self::new(PidConfig::default())
    }
}

impl PidController for Ipd {
    fn new(config: PidConfig) -> Self {
        Self {
            config,
            i_term: 0.0,
            pre_actual: FloatType::NAN,
        }
    }
    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 * actual
            + 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)
    }
}