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

use log::debug;

#[derive(Debug)]
enum ControllerType {
    Proportional,
    Integral,
    Derivative,
}

struct Controller {
    pub controller_type: ControllerType,
    pub gain: f32,
    pub error: f32,
}

impl Controller {
    pub fn new(controller_type: ControllerType, gain: f32) -> Self {
        Self { controller_type, gain, error: 0.0 }
    }

    pub fn update(&mut self, error: f32) {
        self.error = match self.controller_type {
            ControllerType::Proportional => error,
            ControllerType::Integral => (error + self.error) / 2.0,
            ControllerType::Derivative => error - self.error,
        };

        debug!("{:#?}, {}", self.controller_type, self.error);
    }

    pub fn output(&self) -> f32 {
        self.error * self.gain
    }
}

pub struct PidController {
    p: Controller,
    i: Controller,
    d: Controller,
}

impl PidController {
    /// Creates a new PidController with the provided `gain` tuple.
    /// Gain is used to balance the respective volume of each controller.
    pub fn new(gain: (f32, f32, f32)) -> Self {
        let (p_gain, i_gain, d_gain) = gain;
        Self {
            p: Controller::new(ControllerType::Proportional, p_gain),
            i: Controller::new(ControllerType::Integral, i_gain),
            d: Controller::new(ControllerType::Derivative, d_gain),
        }
    }

    pub fn update(&mut self, goal: f32, current: f32) {
        let error = goal - current;

        self.p.update(error);
        self.i.update(error);
        self.d.update(error);

        debug!("PidController, {}", self.output());
    }

    pub fn output(&self) -> f32 {
        self.p.output() + self.i.output() + self.d.output()
    }
}

impl Default for PidController {
    fn default() -> Self {
        Self::new((0.001, 0.0, 0.0))
    }
}