illmatic_scheduling/
pid.rs

1pub struct PidController {
2  pub(crate) set_point: f32,
3  sum: f32,
4  prev: Option<f32>,
5  p_gain: f32,
6  i_gain: f32,
7  d_gain: f32,
8}
9
10impl PidController {
11  pub fn new(p_gain: f32, i_gain: f32, d_gain: f32, set_point: f32) -> Self {
12    Self {
13      set_point,
14      sum: 0.0,
15      prev: None,
16      p_gain,
17      i_gain,
18      d_gain,
19    }
20  }
21  pub fn next(&mut self, observed: f32) -> f32 {
22    let input = self.set_point - observed;
23    let diff: f32;
24    if let Some(prev) = self.prev {
25      diff = input - prev;
26      self.sum += (prev + input) / 2.0;
27    } else {
28      diff = 0.0;
29      self.sum += input;
30    }
31
32    let p = input * self.p_gain;
33    let i = self.sum * self.i_gain;
34    let d  = diff * self.d_gain;
35
36    self.prev = Some(input);
37    p + i + d
38  }
39  pub fn p_gain(&self) -> f32 {
40    self.p_gain
41  }
42  pub fn change_p_gain(&mut self, p_gain: f32) {
43    self.p_gain = p_gain;
44  }
45  pub fn i_gain(&self) -> f32 {
46    self.p_gain
47  }
48  pub fn change_i_gain(&mut self, i_gain: f32) {
49    self.i_gain = i_gain;
50  }
51  pub fn d_gain(&self) -> f32 {
52    self.d_gain
53  }
54  pub fn change_d_gain(&mut self, d_gain: f32) {
55    self.d_gain = d_gain;
56  }
57  pub fn inspect_sum(&self) -> f32 {
58    self.sum
59  }
60}
illmatic_scheduling/pid.rs

illmatic_scheduling/
pid.rs
