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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

use crate::loops::ControlLoop;

/// Ideal DC motor feedforward controller.
///
/// This is a open-loop velocity controller that computes the voltage to
/// maintain an idealized DC motor in a certain state.
///
/// The controller is implemented according to the following model:
///
/// `V = Kₛ sign(ω) + Kᵥ ω + Kₐ α``
pub struct DcMotorFeedforward {
    ks: f64,
    kv: f64,
    ka: f64,

    target_acceleration: f64,
}

impl DcMotorFeedforward {
    /// Creates a new [`DcMotorFeedforward`] with the given constants and target.
    ///
    /// # Parameters
    ///
    /// - `ks` - Feedforward constant for static friction compensation.
    /// - `kv` - Feedforward constant for velocity compensation.
    /// - `ka` - Feedforward constant for acceleration compensation.
    /// - `target_acceleration` - Feedforward constant for the target acceleration.
    pub fn new(ks: f64, kv: f64, ka: f64, target_acceleration: f64) -> Self {
        Self {
            ks,
            kv,
            ka,
            target_acceleration,
        }
    }

    /// Get the current PID gains as a tuple (`ks`, `kv`, `ka`).
    #[must_use]
    pub const fn constants(&self) -> (f64, f64, f64) {
        (self.ks, self.kv, self.ka)
    }

    /// Returns the controller's static friction constant (`ks`).
    #[must_use]
    pub const fn ks(&self) -> f64 {
        self.ks
    }

    /// Returns the controller's velocity constant (`kv`).
    #[must_use]
    pub const fn kv(&self) -> f64 {
        self.kv
    }

    /// Returns the controller's acceleration constant (`kd`).
    #[must_use]
    pub const fn ka(&self) -> f64 {
        self.ka
    }

    /// Sets the PID gains to provided values.
    pub const fn set_constants(&mut self, ks: f64, kv: f64, ka: f64) {
        self.ks = ks;
        self.kv = kv;
        self.ka = ka;
    }

    /// Sets the controller's static friction constant (`ks`).
    pub const fn set_ks(&mut self, ks: f64) {
        self.ks = ks;
    }

    /// Sets the controller's velocity constant (`kv`).
    pub const fn set_kv(&mut self, kv: f64) {
        self.kv = kv;
    }

    /// Sets the controller's acceleration constant (`ka`).
    pub const fn set_ka(&mut self, ka: f64) {
        self.ka = ka;
    }
}

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

    fn update(
        &mut self,
        _measurement: Self::Input,
        setpoint: Self::Input,
        _dt: core::time::Duration,
    ) -> Self::Output {
        self.ks * setpoint.signum() + self.kv * setpoint + self.ka * self.target_acceleration
    }
}