ozton_motion/basic/
turn_to_point.rs

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

ozton_motion/basic/
turn_to_point.rs
