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

use nalgebra::Vector2;
use pid_controller::PD;

#[derive(Clone, Debug)]
pub struct LateralPositionController {
    pd: PD<f32>,
}

impl Default for LateralPositionController {
    fn default() -> Self {
        Self {
            pd: PD {
                p: Default::default(),
                kd: 1.,
            },
        }
    }
}

impl LateralPositionController {
    pub fn lateral_position_control(
        &self,
        local_position_cmd: Vector2<f32>,
        local_velocity_cmd: Vector2<f32>,
        local_position: Vector2<f32>,
        local_velocity: Vector2<f32>,
    ) -> Vector2<f32> {
        self.pd.control(
            local_position_cmd,
            local_velocity_cmd,
            local_position,
            local_velocity,
        )
    }

    pub fn lateral_position_control_with_feed_forward(
        &self,
        local_position_cmd: Vector2<f32>,
        local_velocity_cmd: Vector2<f32>,
        local_position: Vector2<f32>,
        local_velocity: Vector2<f32>,
        acceleration_ff: Vector2<f32>,
    ) -> Vector2<f32> {
        self.lateral_position_control(
            local_position_cmd,
            local_velocity_cmd,
            local_position,
            local_velocity,
        ) + acceleration_ff
    }
}