carla_ackermann/
accel_control.rs

1use crate::{physics::VehiclePhysics, pid::PidInit};
2use pid::Pid;
3
4#[derive(Debug, Clone)]
5pub struct AccelControllerInit {
6    pub pid: PidInit,
7    pub max_pedal: f64,
8}
9
10impl AccelControllerInit {
11    pub fn from_physics(physics: &VehiclePhysics) -> Self {
12        Self {
13            pid: PidInit {
14                kp: 0.05,
15                ki: 0.0,
16                kd: 0.05,
17                output_limit: 1.0,
18            },
19            max_pedal: physics.max_accel().min(physics.max_deceleration()),
20        }
21    }
22
23    pub fn build(&self) -> AccelController {
24        let Self { ref pid, max_pedal } = *self;
25        AccelController {
26            accel_pid: pid.build(),
27            target_accel: 0.0,
28            target_pedal: 0.0,
29            max_pedal,
30        }
31    }
32}
33
34#[derive(Debug)]
35pub struct AccelController {
36    accel_pid: Pid<f64>,
37    target_accel: f64,
38    target_pedal: f64,
39    max_pedal: f64,
40}
41
42impl AccelController {
43    pub fn set_target_accel(&mut self, target_accel: f64) {
44        self.target_accel = target_accel;
45    }
46
47    pub fn reset_target_pedal(&mut self) {
48        self.target_pedal = 0.0;
49    }
50
51    pub fn step(&mut self, current_accel: f64) -> AccelControl {
52        let Self {
53            ref mut accel_pid,
54            target_pedal: prev_target_pedal,
55            max_pedal,
56            target_accel,
57        } = *self;
58
59        accel_pid.setpoint = target_accel;
60        let pedal_delta = accel_pid.next_control_output(current_accel).output;
61        let curr_pedal_target = (prev_target_pedal + pedal_delta).clamp(-max_pedal, max_pedal);
62        self.target_pedal = curr_pedal_target;
63
64        AccelControl {
65            target_pedal: curr_pedal_target,
66            pedal_delta,
67        }
68    }
69
70    pub fn max_pedal(&self) -> f64 {
71        self.max_pedal
72    }
73}
74
75pub struct AccelControl {
76    pub target_pedal: f64,
77    pub pedal_delta: f64,
78}
carla_ackermann/accel_control.rs

carla_ackermann/
accel_control.rs
