evian_motion/seeking/
move_to_point.rs

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

evian_motion/seeking/
move_to_point.rs
