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 {
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))
}
}