kcan 0.1.4

CAN controller primitives for actuator and motor control.
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112

use kcan::{LowPassFilterConfig, PIDConfig, PIDController, SecondOrderLowPassFilter, SolverType};

fn assert_close(left: f64, right: f64, tolerance: f64) {
    assert!(
        (left - right).abs() <= tolerance,
        "left={left} right={right} tolerance={tolerance}"
    );
}

#[test]
fn pid_first_output_is_zero() {
    let mut pid = PIDController::new(PIDConfig::default(), LowPassFilterConfig::default());

    assert_eq!(pid.compute_output(1.0, 1.0, 0.0), 0.0);
}

#[test]
fn pid_proportional_term_only() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 2.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-100.0, 100.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    let output = pid.compute_output(1.0, 1.0, 0.5);

    assert_close(output, 1.0, f64::EPSILON);
}

#[test]
fn pid_clamps_large_error_to_sixty_degrees() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 1.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-100.0, 100.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    let output = pid.compute_output(1.0, 500_f64.to_radians(), 0.0);

    assert_close(output, 60_f64.to_radians(), 1e-12);
}

#[test]
fn pid_output_is_clamped() {
    let mut pid = PIDController::new(
        PIDConfig {
            proportional_gain: 1000.0,
            integral_gain: 0.0,
            derivative_gain: 0.0,
            output_limits: Some((-5.0, 5.0)),
        },
        LowPassFilterConfig::default(),
    );

    pid.compute_output(0.0, 0.0, 0.0);
    assert_eq!(pid.compute_output(1.0, 1.0, 0.0), 5.0);
}

#[test]
fn pid_reset_clears_internal_state() {
    let mut pid = PIDController::default();
    pid.compute_output(0.0, 1.0, 0.0);
    pid.compute_output(1.0, 1.0, 0.0);

    pid.reset();

    assert_eq!(pid.integral(), 0.0);
    assert_eq!(pid.previous_velocity(), 0.0);
}

#[test]
fn low_pass_filter_converges_to_constant_input() {
    let mut filter = SecondOrderLowPassFilter::new(LowPassFilterConfig {
        cut_off_frequency_rad_per_sec: 20.0,
        damping_ratio: 1.0,
        initial_condition: 0.0,
        solver_type: SolverType::RungeKutta,
    });

    let mut output = 0.0;
    for _ in 0..500 {
        (output, _) = filter.step(1.0, 0.01);
    }

    assert_close(output, 1.0, 1e-2);
}

#[test]
fn low_pass_filter_reset_restores_initial_condition() {
    let mut filter = SecondOrderLowPassFilter::new(LowPassFilterConfig {
        cut_off_frequency_rad_per_sec: 20.0,
        damping_ratio: 1.0,
        initial_condition: 2.5,
        solver_type: SolverType::ForwardEuler,
    });

    filter.step(10.0, 0.01);
    filter.reset();

    assert_close(filter.state().filtered_output, 2.5, f64::EPSILON);
    assert_close(filter.state().feedback_state, 2.5, f64::EPSILON);
}