evian-control 0.0.1-rc.1

Controls theory primitives in evian.
Documentation 
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

use core::time::Duration;

use crate::loops::ControlLoop;

/// Take-back-half flywheel velocity controller.
pub struct TakeBackHalf {
    kh: f64,
    tbh: f64,
    integral: f64,
    prev_error: f64,
}

impl TakeBackHalf {
    /// Creates a new TBH controller.
    pub fn new(kh: f64) -> Self {
        Self {
            kh,
            tbh: 0.0,
            integral: 0.0,
            prev_error: 0.0,
        }
    }

    /// Returns the controller's integral gain (`kh`).
    pub fn kh(&self) -> f64 {
        self.kh
    }

    /// Sets the controller's integral gain (`kh`).
    pub fn set_kh(&mut self, kh: f64) {
        self.kh = kh;
    }
}

impl ControlLoop for TakeBackHalf {
    type Input = f64;
    type Output = f64;

    fn update(
        &mut self,
        measurement: Self::Input,
        setpoint: Self::Input,
        _dt: Duration,
    ) -> Self::Output {
        let error = setpoint - measurement;

        self.integral += error * self.kh;

        if error * self.prev_error <= 0.0 {
            self.integral = 0.5 * (self.integral + self.tbh);
            self.tbh = self.integral;
        }

        self.prev_error = error;

        self.tbh
    }
}