carla_ackermann/
speed_control.rs

1use crate::{
2    constants::{FULL_STOP_SPEED_MS, INTERNAL_ACCEL_MS2, STAND_STILL_SPEED_MS},
3    physics::VehiclePhysics,
4    pid::PidInit,
5};
6use pid::Pid;
7
8#[derive(Debug, Clone)]
9pub struct SpeedControllerInit {
10    pub pid: PidInit,
11    pub max_speed: f64,
12    pub max_accel: f64,
13    pub min_accel: f64,
14    pub max_decel: f64,
15}
16
17impl SpeedControllerInit {
18    pub fn from_physics(physics: &VehiclePhysics, min_accel: Option<f64>) -> Self {
19        Self {
20            pid: PidInit {
21                kp: 0.05,
22                ki: 0.0,
23                kd: 0.5,
24                output_limit: 1.0,
25            },
26            max_speed: physics.max_speed(),
27            max_accel: physics.max_accel(),
28            min_accel: min_accel.unwrap_or(1.0),
29            max_decel: physics.max_deceleration(),
30        }
31    }
32
33    pub fn build(&self) -> SpeedController {
34        let Self {
35            ref pid,
36            max_speed,
37            max_accel,
38            min_accel,
39            max_decel,
40        } = *self;
41
42        SpeedController {
43            speed_pid: pid.build(),
44            accel_activator: DelayedActivator::new(5),
45            target_speed: 0.0,
46            target_accel: 0.0,
47            max_speed,
48            max_accel,
49            min_accel,
50            max_decel,
51        }
52    }
53}
54
55#[derive(Debug)]
56pub struct SpeedController {
57    speed_pid: Pid<f64>,
58    accel_activator: DelayedActivator,
59    target_speed: f64,
60    target_accel: f64,
61    max_speed: f64,
62    max_accel: f64,
63    min_accel: f64,
64    max_decel: f64,
65}
66
67impl SpeedController {
68    pub fn target_speed(&self) -> f64 {
69        self.target_speed
70    }
71
72    pub fn set_target(&mut self, target_speed: f64, target_accel: f64) {
73        let Self {
74            max_speed,
75            max_accel,
76            max_decel,
77            ..
78        } = *self;
79        let target_speed = target_speed.clamp(-max_speed, max_speed);
80        let target_accel = if target_speed.abs() >= FULL_STOP_SPEED_MS {
81            target_accel.clamp(-max_decel, max_accel)
82        } else {
83            -max_decel
84        };
85
86        self.target_speed = target_speed;
87        self.target_accel = target_accel;
88    }
89
90    pub fn step(&mut self, current_speed: f64) -> SpeedControl {
91        let Self {
92            ref mut speed_pid,
93            // ref mut accel_activator,
94            target_speed,
95            target_accel,
96            // min_accel,
97            max_accel,
98            max_decel,
99            ..
100        } = *self;
101
102        let is_standing = current_speed.abs() < STAND_STILL_SPEED_MS;
103        let is_stopping = target_speed.abs() < FULL_STOP_SPEED_MS;
104        let is_full_stop = is_standing && is_stopping;
105
106        let setpoint_speed = match (is_standing, is_stopping) {
107            (true, true) => 0.0,
108            (true, false) => target_speed,
109            _ => {
110                if current_speed.is_sign_positive() ^ target_speed.is_sign_positive() {
111                    0.0
112                } else {
113                    target_speed
114                }
115            }
116        };
117
118        let target_accel_abs = target_accel.abs();
119        let is_inertial = target_accel_abs < INTERNAL_ACCEL_MS2;
120        // let is_speed_control_enabled = {
121        //     let is_accel_triggered = !is_inertial && target_accel_abs >= min_accel;
122
123        //     if is_accel_triggered {
124        //         accel_activator.inc()
125        //     } else {
126        //         accel_activator.dec();
127        //         false
128        //     }
129        // };
130        let is_speed_control_enabled = true;
131
132        let (setpoint_accel, delta_accel) = if is_speed_control_enabled {
133            speed_pid.setpoint = setpoint_speed.abs();
134            let delta = speed_pid.next_control_output(current_speed).output;
135
136            let (lower, upper) = if is_inertial {
137                (-max_decel, max_accel)
138            } else {
139                (-target_accel_abs, target_accel_abs)
140            };
141
142            let prev_target = if is_full_stop { 0.0 } else { target_accel };
143            let target = (prev_target + delta).clamp(lower, upper);
144            (target, delta)
145        } else {
146            (target_accel, 0.0)
147        };
148
149        SpeedControl {
150            setpoint_accel,
151            delta_accel,
152            full_stop: is_full_stop,
153        }
154    }
155}
156
157pub struct SpeedControl {
158    pub setpoint_accel: f64,
159    pub delta_accel: f64,
160    pub full_stop: bool,
161}
162
163#[derive(Debug)]
164struct DelayedActivator {
165    max: usize,
166    cur: usize,
167}
168
169impl DelayedActivator {
170    pub fn new(max: usize) -> Self {
171        Self { max, cur: 0 }
172    }
173
174    pub fn inc(&mut self) -> bool {
175        let Self { max, cur } = *self;
176        let next = if max == cur { max } else { cur + 1 };
177        self.cur = next;
178        next == max
179    }
180
181    pub fn dec(&mut self) {
182        if let Some(next) = self.cur.checked_sub(1) {
183            self.cur = next;
184        }
185    }
186}
carla_ackermann/speed_control.rs

carla_ackermann/
speed_control.rs
