evian_motion/basic/
distance_at_heading.rs

1use core::{
2    future::Future,
3    pin::Pin,
4    task::{Context, Poll},
5    time::Duration,
6};
7
8use vexide::{
9    devices::smart::Motor,
10    time::{Instant, Sleep, sleep},
11};
12
13use evian_control::{
14    Tolerances,
15    loops::{AngularPid, ControlLoop, Pid},
16};
17use evian_drivetrain::{
18    Drivetrain,
19    differential::{Differential, Voltages},
20};
21use evian_math::Angle;
22use evian_tracking::{TracksForwardTravel, TracksHeading, TracksVelocity};
23
24pub(crate) struct State {
25    sleep: Sleep,
26    initial_forward_travel: f64,
27    start_time: Instant,
28    prev_time: Instant,
29    linear_settled: bool,
30    angular_settled: bool,
31}
32
33/// Drives the robot forward or backwards for a distance at a given heading.
34pub struct DriveDistanceAtHeadingFuture<
35    'a,
36    L: ControlLoop<Input = f64, Output = f64> + Unpin,
37    A: ControlLoop<Input = Angle, Output = f64> + Unpin,
38    T: TracksForwardTravel + TracksHeading + TracksVelocity,
39> {
40    pub(crate) target_distance: f64,
41    pub(crate) target_heading: Angle,
42    pub(crate) timeout: Option<Duration>,
43    pub(crate) linear_tolerances: Tolerances,
44    pub(crate) angular_tolerances: Tolerances,
45    pub(crate) linear_controller: L,
46    pub(crate) angular_controller: A,
47    pub(crate) drivetrain: &'a mut Drivetrain<Differential, T>,
48
49    /// Internal future state ("local variables").
50    pub(crate) state: Option<State>,
51}
52
53// MARK: Future Poll
54
55impl<
56    L: ControlLoop<Input = f64, Output = f64> + Unpin,
57    A: ControlLoop<Input = Angle, Output = f64> + Unpin,
58    T: TracksForwardTravel + TracksHeading + TracksVelocity,
59> Future for DriveDistanceAtHeadingFuture<'_, L, A, T>
60{
61    type Output = ();
62
63    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
64        let this = self.get_mut();
65        let state = this.state.get_or_insert_with(|| {
66            let now = Instant::now();
67            State {
68                sleep: sleep(Duration::from_millis(5)),
69                initial_forward_travel: this.drivetrain.tracking.forward_travel(),
70                start_time: now,
71                prev_time: now,
72                linear_settled: false,
73                angular_settled: false,
74            }
75        });
76
77        if Pin::new(&mut state.sleep).poll(cx).is_pending() {
78            return Poll::Pending;
79        }
80
81        let dt = state.prev_time.elapsed();
82
83        let forward_travel = this.drivetrain.tracking.forward_travel();
84        let heading = this.drivetrain.tracking.heading();
85
86        let linear_error = (this.target_distance + state.initial_forward_travel) - forward_travel;
87        let angular_error = (this.target_heading - heading).wrapped();
88
89        if this
90            .linear_tolerances
91            .check(linear_error, this.drivetrain.tracking.linear_velocity())
92        {
93            state.linear_settled = true;
94        }
95        if this.angular_tolerances.check(
96            angular_error.as_radians(),
97            this.drivetrain.tracking.angular_velocity(),
98        ) {
99            state.angular_settled = true;
100        }
101
102        if (state.linear_settled && state.angular_settled)
103            || this
104                .timeout
105                .is_some_and(|timeout| state.start_time.elapsed() > timeout)
106        {
107            _ = this.drivetrain.motors.set_voltages((0.0, 0.0));
108            return Poll::Ready(());
109        }
110
111        let linear_output = this.linear_controller.update(
112            forward_travel,
113            this.target_distance + state.initial_forward_travel,
114            dt,
115        );
116        let angular_output = this
117            .angular_controller
118            .update(heading, this.target_heading, dt);
119
120        _ = this.drivetrain.motors.set_voltages(
121            Voltages(
122                linear_output + angular_output,
123                linear_output - angular_output,
124            )
125            .normalized(Motor::V5_MAX_VOLTAGE),
126        );
127
128        state.sleep = sleep(Duration::from_millis(5));
129        state.prev_time = Instant::now();
130
131        cx.waker().wake_by_ref();
132        Poll::Pending
133    }
134}
135
136// MARK: Generic Modifiers
137
138impl<
139    L: ControlLoop<Input = f64, Output = f64> + Unpin,
140    A: ControlLoop<Input = Angle, Output = f64> + Unpin,
141    T: TracksForwardTravel + TracksHeading + TracksVelocity,
142> DriveDistanceAtHeadingFuture<'_, L, A, T>
143{
144    /// Modifies this motion's linear feedback controller.
145    pub fn with_linear_controller(&mut self, controller: L) -> &mut Self {
146        self.linear_controller = controller;
147        self
148    }
149
150    /// Modifies this motion's angular feedback controller.
151    pub fn with_angular_controller(&mut self, controller: A) -> &mut Self {
152        self.angular_controller = controller;
153        self
154    }
155
156    /// Modifies this motion's timeout duration.
157    pub const fn with_timeout(&mut self, timeout: Duration) -> &mut Self {
158        self.timeout = Some(timeout);
159        self
160    }
161
162    /// Removes this motion's timeout duration.
163    pub const fn without_timeout(&mut self) -> &mut Self {
164        self.timeout = None;
165        self
166    }
167
168    /// Modifies this motion's linear tolerances.
169    pub const fn with_linear_tolerances(&mut self, tolerances: Tolerances) -> &mut Self {
170        self.linear_tolerances = tolerances;
171        self
172    }
173
174    /// Modifies this motion's linear error tolerance.
175    pub const fn with_linear_error_tolerance(&mut self, tolerance: f64) -> &mut Self {
176        self.linear_tolerances.error_tolerance = Some(tolerance);
177        self
178    }
179
180    /// Removes this motion's linear error tolerance.
181    pub const fn without_linear_error_tolerance(&mut self) -> &mut Self {
182        self.linear_tolerances.error_tolerance = None;
183        self
184    }
185
186    /// Modifies this motion's linear velocity tolerance.
187    pub const fn with_linear_velocity_tolerance(&mut self, tolerance: f64) -> &mut Self {
188        self.linear_tolerances.velocity_tolerance = Some(tolerance);
189        self
190    }
191
192    /// Removes this motion's linear velocity tolerance.
193    pub const fn without_linear_velocity_tolerance(&mut self) -> &mut Self {
194        self.linear_tolerances.velocity_tolerance = None;
195        self
196    }
197
198    /// Modifies this motion's linear tolerance duration.
199    pub const fn with_linear_tolerance_duration(&mut self, duration: Duration) -> &mut Self {
200        self.linear_tolerances.duration = Some(duration);
201        self
202    }
203
204    /// Removes this motion's linear tolerance duration.
205    pub const fn without_linear_tolerance_duration(&mut self) -> &mut Self {
206        self.linear_tolerances.duration = None;
207        self
208    }
209
210    /// Modifies this motion's angular tolerances.
211    pub const fn with_angular_tolerances(&mut self, tolerances: Tolerances) -> &mut Self {
212        self.angular_tolerances = tolerances;
213        self
214    }
215
216    /// Modifies this motion's angular error tolerance.
217    pub const fn with_angular_error_tolerance(&mut self, tolerance: f64) -> &mut Self {
218        self.angular_tolerances.error_tolerance = Some(tolerance);
219        self
220    }
221
222    /// Removes this motion's angular error tolerance.
223    pub const fn without_angular_error_tolerance(&mut self) -> &mut Self {
224        self.angular_tolerances.error_tolerance = None;
225        self
226    }
227
228    /// Modifies this motion's angular velocity tolerance.
229    pub const fn with_angular_velocity_tolerance(&mut self, tolerance: f64) -> &mut Self {
230        self.angular_tolerances.velocity_tolerance = Some(tolerance);
231        self
232    }
233
234    /// Removes this motion's angular velocity tolerance.
235    pub const fn without_angular_velocity_tolerance(&mut self) -> &mut Self {
236        self.angular_tolerances.velocity_tolerance = None;
237        self
238    }
239
240    /// Modifies this motion's angular tolerance duration.
241    pub const fn with_angular_tolerance_duration(&mut self, duration: Duration) -> &mut Self {
242        self.angular_tolerances.duration = Some(duration);
243        self
244    }
245
246    /// Removes this motion's angular tolerance duration.
247    pub const fn without_angular_tolerance_duration(&mut self) -> &mut Self {
248        self.angular_tolerances.duration = None;
249        self
250    }
251}
252
253// MARK: Linear PID Modifiers
254
255impl<
256    A: ControlLoop<Input = Angle, Output = f64> + Unpin,
257    T: TracksForwardTravel + TracksHeading + TracksVelocity,
258> DriveDistanceAtHeadingFuture<'_, Pid, A, T>
259{
260    /// Modifies this motion's linear PID gains.
261    pub const fn with_linear_gains(&mut self, kp: f64, ki: f64, kd: f64) -> &mut Self {
262        self.linear_controller.set_gains(kp, ki, kd);
263        self
264    }
265
266    /// Modifies this motion's linear proportional gain (`kp`).
267    pub const fn with_linear_kp(&mut self, kp: f64) -> &mut Self {
268        self.linear_controller.set_kp(kp);
269        self
270    }
271
272    /// Modifies this motion's linear integral gain (`ki`).
273    pub const fn with_linear_ki(&mut self, ki: f64) -> &mut Self {
274        self.linear_controller.set_ki(ki);
275        self
276    }
277
278    /// Modifies this motion's linear derivative gain (`kd`).
279    pub const fn with_linear_kd(&mut self, kd: f64) -> &mut Self {
280        self.linear_controller.set_kd(kd);
281        self
282    }
283
284    /// Modifies this motion's linear integration range.
285    pub const fn with_linear_integration_range(&mut self, integration_range: f64) -> &mut Self {
286        self.linear_controller
287            .set_integration_range(Some(integration_range));
288        self
289    }
290
291    /// Removes this motion's linear integration range.
292    pub const fn without_linear_integration_range(&mut self) -> &mut Self {
293        self.linear_controller.set_integration_range(None);
294        self
295    }
296
297    /// Modifies this motion's linear output limit.
298    pub const fn with_linear_output_limit(&mut self, limit: f64) -> &mut Self {
299        self.linear_controller.set_output_limit(Some(limit));
300        self
301    }
302
303    /// Removes this motion's linear output limit.
304    pub const fn without_linear_output_limit(&mut self) -> &mut Self {
305        self.linear_controller.set_output_limit(None);
306        self
307    }
308}
309
310// MARK: Angular PID Modifiers
311
312impl<
313    L: ControlLoop<Input = f64, Output = f64> + Unpin,
314    T: TracksForwardTravel + TracksHeading + TracksVelocity,
315> DriveDistanceAtHeadingFuture<'_, L, AngularPid, T>
316{
317    /// Modifies this motion's angular PID gains.
318    pub const fn with_angular_gains(&mut self, kp: f64, ki: f64, kd: f64) -> &mut Self {
319        self.angular_controller.set_gains(kp, ki, kd);
320        self
321    }
322
323    /// Modifies this motion's angular proportional gain (`kp`).
324    pub const fn with_angular_kp(&mut self, kp: f64) -> &mut Self {
325        self.angular_controller.set_kp(kp);
326        self
327    }
328
329    /// Modifies this motion's angular integral gain (`ki`).
330    pub const fn with_angular_ki(&mut self, ki: f64) -> &mut Self {
331        self.angular_controller.set_ki(ki);
332        self
333    }
334
335    /// Modifies this motion's angular derivative gain (`kd`).
336    pub const fn with_angular_kd(&mut self, kd: f64) -> &mut Self {
337        self.angular_controller.set_kd(kd);
338        self
339    }
340
341    /// Modifies this motion's angular integration range.
342    pub const fn with_angular_integration_range(&mut self, integration_range: Angle) -> &mut Self {
343        self.angular_controller
344            .set_integration_range(Some(integration_range));
345        self
346    }
347
348    /// Modifies this motion's angular output limit.
349    pub const fn with_angular_output_limit(&mut self, limit: f64) -> &mut Self {
350        self.angular_controller.set_output_limit(Some(limit));
351        self
352    }
353
354    /// Removes this motion's angular integration range.
355    pub const fn without_angular_integration_range(&mut self) -> &mut Self {
356        self.angular_controller.set_integration_range(None);
357        self
358    }
359
360    /// Removes this motion's angular output limit.
361    pub const fn without_angular_output_limit(&mut self) -> &mut Self {
362        self.angular_controller.set_output_limit(None);
363        self
364    }
365}
evian_motion/basic/distance_at_heading.rs

evian_motion/basic/
distance_at_heading.rs
