fll_rs/movement/
pid.rs

1#[derive(Copy, Clone, Debug)]
2pub struct PidConfig {
3    /// How strongly to correct current error
4    pub kp: f32,
5
6    /// How strongly to correct to long term drift
7    pub ki: f32,
8
9    /// How strongly to correct for predicted error
10    pub kd: f32,
11
12    /// The max allowed value for the integral
13    pub max_i: f32,
14}
15
16/// Implementation of PID algorithm
17#[derive(Clone, Debug)]
18pub struct PidController {
19    pid: PidConfig,
20
21    integral: f32,
22    last_error: f32,
23}
24
25impl PidController {
26    pub fn new(pid: PidConfig) -> Self {
27        PidController {
28            pid,
29            integral: 0.0,
30            last_error: 0.0,
31        }
32    }
33
34    pub fn update(&mut self, error: f32) -> (f32, (f32, f32, f32)) {
35        let cfg = &self.pid;
36
37        self.integral += error;
38        self.integral = self.integral.clamp(-cfg.max_i, cfg.max_i);
39
40        let proportional = error;
41        let integral = self.integral;
42        let derivative = error - self.last_error;
43
44        self.last_error = error;
45
46        let p = cfg.kp * proportional;
47        let i = cfg.ki * integral;
48        let d = cfg.kd * derivative;
49
50        (p + i + d, (p, i, d))
51    }
52}
fll_rs/movement/pid.rs

fll_rs/movement/
pid.rs
