discrete_pid 0.2.0

A PID controller for robotics and discrete control systems
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

use crate::time::{InstantLike, Millis};
use nalgebra as na;

pub fn rk4_step<const N: usize>(
    f: impl Fn(na::SVector<f64, N>) -> na::SVector<f64, N>,
    x0: na::SVector<f64, N>,
    dt: f64,
) -> na::SVector<f64, N> {
    let k1 = f(x0);
    let k2 = f(x0 + k1 * dt / 2.0);
    let k3 = f(x0 + k2 * dt / 2.0);
    let k4 = f(x0 + k3 * dt);
    x0 + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * dt / 6.0
}

pub enum WaveForm {
    Sine,
    Square,
}

pub struct SignalGenerator<I: InstantLike> {
    fcn: fn(f64) -> f64,
    initial_time: I,
    amplitude: f64,
    offset: f64,
}

impl<I: InstantLike> SignalGenerator<I> {
    pub fn new(waveform: WaveForm, initial_time: I, amplitude: f64, offset: f64) -> Self {
        Self {
            fcn: match waveform {
                WaveForm::Sine => f64::sin,
                WaveForm::Square => |x| x.sin().signum(),
            },
            initial_time,
            amplitude,
            offset,
        }
    }

    pub fn generate(&self, time: I) -> f64 {
        self.amplitude * (self.fcn)(time.duration_since(self.initial_time).as_secs_f64())
            + self.offset
    }
}

pub fn sine_signal(time: Millis, initial_time: Millis) -> f64 {
    time.duration_since(initial_time).as_secs_f64().sin()
}

pub fn square_signal(time: Millis, initial_time: Millis) -> f64 {
    time.duration_since(initial_time)
        .as_secs_f64()
        .sin()
        .signum()
}

pub struct MassSpringDamper {
    pub natural_frequency: f64,
    pub damping_ratio: f64,
}

impl MassSpringDamper {
    /// Implements the state-space realization of the mass-spring-damper system:
    /// ┌     ┐   ┌              ┐┌    ┐   ┌     ┐
    /// │ p'  │ = │  0     1     ││ p  │ + │ 0   │ u
    /// │ p'' │   │  -ωₙ²  -2ζωₙ ││ p' │   │ ωₙ² │
    /// └     ┘   └              ┘└    ┘   └     ┘
    ///     ┌      ┐┌    ┐
    /// p = │ 1  0 ││ p  │         
    ///     └      ┘│ p' │
    ///             └    ┘
    pub fn f(&self, x: na::Vector2<f64>, u: f64) -> na::Vector2<f64> {
        let omega_sq = self.natural_frequency.powi(2);
        let two_zeta_omega = 2.0 * self.natural_frequency * self.damping_ratio;

        let mat_a = na::Matrix2::new(0.0, 1.0, -omega_sq, -two_zeta_omega);
        let mat_b = na::Vector2::new(0.0, omega_sq);

        mat_a * x + mat_b * u
    }

    pub fn h(&self, x: na::Vector2<f64>) -> f64 {
        x[0]
    }
}