diff --git a/packages/evian-control/src/loops/bang_bang.rs b/packages/evian-control/src/loops/bang_bang.rs index 64b248a..5d22553 100644 --- a/packages/evian-control/src/loops/bang_bang.rs +++ b/packages/evian-control/src/loops/bang_bang.rs @@ -34,7 +34,7 @@ impl ControlLoop for BangBang { } impl Feedback for BangBang { - fn update(&mut self, measurement: f64, setpoint: f64, _dt: Duration) -> f64 { + fn update(&mut self, measurement: f64, setpoint: f64, _dt: Option) -> f64 { if measurement < setpoint { self.magnitude } else { diff --git a/packages/evian-control/src/loops/cascade.rs b/packages/evian-control/src/loops/cascade.rs index 016a4d3..d90a212 100644 --- a/packages/evian-control/src/loops/cascade.rs +++ b/packages/evian-control/src/loops/cascade.rs @@ -60,7 +60,7 @@ impl> Cascade { primary_measurement: P::Input, secondary_measurement: S::Input, setpoint: P::Input, - dt: Duration, + dt: Option, ) -> S::Output { self.secondary.update( secondary_measurement, @@ -149,7 +149,12 @@ impl Feedback for Cascade where Fb::Input: Clone, { - fn update(&mut self, measurement: Fb::Input, setpoint: Fb::Input, dt: Duration) -> Ff::Output { + fn update( + &mut self, + measurement: Fb::Input, + setpoint: Fb::Input, + dt: Option, + ) -> Ff::Output { self.secondary.update( // SAFETY: This variant of the struct may only be constructed with Some(fn). (unsafe { self.map_fn.unwrap_unchecked() })( diff --git a/packages/evian-control/src/loops/feedforward.rs b/packages/evian-control/src/loops/feedforward.rs index a579ffa..c5c376d 100644 --- a/packages/evian-control/src/loops/feedforward.rs +++ b/packages/evian-control/src/loops/feedforward.rs @@ -100,7 +100,11 @@ impl ControlLoop for MotorFeedforward { } impl Feedforward for MotorFeedforward { - fn update(&mut self, setpoint: MotorFeedforwardSetpoint, _dt: core::time::Duration) -> f64 { + fn update( + &mut self, + setpoint: MotorFeedforwardSetpoint, + _dt: Option, + ) -> f64 { self.ks * setpoint.velocity.signum() + self.kv * setpoint.velocity + self.ka * setpoint.acceleration @@ -202,7 +206,11 @@ impl ControlLoop for ArmFeedforward { } impl Feedforward for ArmFeedforward { - fn update(&mut self, setpoint: ArmFeedforwardSetpoint, _dt: core::time::Duration) -> f64 { + fn update( + &mut self, + setpoint: ArmFeedforwardSetpoint, + _dt: Option, + ) -> f64 { self.kg * setpoint.position.cos() + self.ks * setpoint.velocity.signum() + self.kv * setpoint.velocity @@ -296,7 +304,11 @@ impl ControlLoop for ElevatorFeedforward { } impl Feedforward for ElevatorFeedforward { - fn update(&mut self, setpoint: ElevatorFeedforwardSetpoint, _dt: core::time::Duration) -> f64 { + fn update( + &mut self, + setpoint: ElevatorFeedforwardSetpoint, + _dt: Option, + ) -> f64 { self.kg + self.ks * setpoint.velocity.signum() + self.kv * setpoint.velocity diff --git a/packages/evian-control/src/loops/mod.rs b/packages/evian-control/src/loops/mod.rs index 9323350..542f307 100644 --- a/packages/evian-control/src/loops/mod.rs +++ b/packages/evian-control/src/loops/mod.rs @@ -64,12 +64,12 @@ pub trait Feedback: ControlLoop { &mut self, measurement: Self::Input, setpoint: Self::Input, - dt: Duration, + dt: Option, ) -> Self::Output; } /// Feedforward ("open-loop") controller. pub trait Feedforward: ControlLoop { /// Updates the feedforward controller's setpoint, producing a new control signal. - fn update(&mut self, setpoint: Self::Input, dt: Duration) -> Self::Output; + fn update(&mut self, setpoint: Self::Input, dt: Option) -> Self::Output; } diff --git a/packages/evian-control/src/loops/pid.rs b/packages/evian-control/src/loops/pid.rs index b7c01f8..c9c145d 100644 --- a/packages/evian-control/src/loops/pid.rs +++ b/packages/evian-control/src/loops/pid.rs @@ -202,7 +202,7 @@ impl ControlLoop for Pid { } impl Feedback for Pid { - fn update(&mut self, measurement: f64, setpoint: f64, dt: Duration) -> f64 { + fn update(&mut self, measurement: f64, setpoint: f64, dt: Option) -> f64 { let error = setpoint - measurement; // If an integration range is used and we are within it, add to the integral. @@ -212,13 +212,16 @@ impl Feedback for Pid { .is_none_or(|range| error.abs() < range) && error.signum() == self.prev_error.signum() { - self.integral += error * dt.as_secs_f64(); + self.integral += error * dt.map(|dt| dt.as_secs_f64()).unwrap_or(0.0); } else { self.integral = 0.0; } // Calculate derivative (change in error / change in time) - let derivative = (error - self.prev_error) / dt.as_secs_f64(); + let derivative = match dt { + Some(dt) => (error - self.prev_error) / dt.as_secs_f64(), + None => 0.0, + }; self.prev_error = error; // Control signal = error * kp + integral + ki + derivative * kd. @@ -359,7 +362,7 @@ impl ControlLoop for AngularPid { } impl Feedback for AngularPid { - fn update(&mut self, measurement: Angle, setpoint: Angle, dt: Duration) -> f64 { + fn update(&mut self, measurement: Angle, setpoint: Angle, dt: Option) -> f64 { let error = (setpoint - measurement).wrapped(); // If an integration range is used and we are within it, add to the integral. @@ -370,13 +373,16 @@ impl Feedback for AngularPid { .is_none_or(|range| error.as_radians().abs() < range.as_radians()) && error.signum() == self.prev_error.signum() { - self.integral += error.as_radians() * dt.as_secs_f64(); + self.integral += error.as_radians() * dt.map(|dt| dt.as_secs_f64()).unwrap_or(0.0); } else { self.integral = 0.0; } // Calculate derivative (change in error / change in time) - let derivative = (error - self.prev_error).as_radians() / dt.as_secs_f64(); + let derivative = match dt { + Some(dt) => (error - self.prev_error).as_radians() / dt.as_secs_f64(), + None => 0.0, + }; self.prev_error = error; let mut output = diff --git a/packages/evian-control/src/loops/tbh.rs b/packages/evian-control/src/loops/tbh.rs index e683e1d..76c8b0a 100644 --- a/packages/evian-control/src/loops/tbh.rs +++ b/packages/evian-control/src/loops/tbh.rs @@ -42,7 +42,7 @@ impl ControlLoop for TakeBackHalf { } impl Feedback for TakeBackHalf { - fn update(&mut self, measurement: f64, setpoint: f64, _dt: Duration) -> f64 { + fn update(&mut self, measurement: f64, setpoint: f64, _dt: Option) -> f64 { let error = setpoint - measurement; self.integral += error * self.kh; diff --git a/packages/evian-motion/src/basic/distance_at_heading.rs b/packages/evian-motion/src/basic/distance_at_heading.rs index dd97a7d..4ed7d35 100644 --- a/packages/evian-motion/src/basic/distance_at_heading.rs +++ b/packages/evian-motion/src/basic/distance_at_heading.rs @@ -19,7 +19,7 @@ pub(crate) struct State { sleep: Sleep, initial_forward_travel: f64, start_time: Instant, - prev_time: Instant, + prev_time: Option, linear_settled: bool, angular_settled: bool, } @@ -65,7 +65,7 @@ where sleep: sleep(Duration::from_millis(5)), initial_forward_travel: this.drivetrain.tracking.forward_travel(), start_time: now, - prev_time: now, + prev_time: None, linear_settled: false, angular_settled: false, } @@ -75,7 +75,7 @@ where return Poll::Pending; } - let dt = state.prev_time.elapsed(); + let dt = state.prev_time.map(|pt| pt.elapsed()); let forward_travel = this.drivetrain.tracking.forward_travel(); let heading = this.drivetrain.tracking.heading(); @@ -114,13 +114,14 @@ where .angular_controller .update(heading, this.target_heading, dt); - drop(this - .drivetrain - .model - .drive_arcade(linear_output, angular_output)); + drop( + this.drivetrain + .model + .drive_arcade(linear_output, angular_output), + ); state.sleep = sleep(Duration::from_millis(5)); - state.prev_time = Instant::now(); + state.prev_time = Some(Instant::now()); cx.waker().wake_by_ref(); Poll::Pending diff --git a/packages/evian-motion/src/basic/turn_to_point.rs b/packages/evian-motion/src/basic/turn_to_point.rs index 2053ae3..72adf0f 100644 --- a/packages/evian-motion/src/basic/turn_to_point.rs +++ b/packages/evian-motion/src/basic/turn_to_point.rs @@ -19,7 +19,7 @@ pub(crate) struct State { sleep: Sleep, initial_forward_travel: f64, start_time: Instant, - prev_time: Instant, + prev_time: Option, linear_settled: bool, angular_settled: bool, } @@ -64,7 +64,7 @@ where sleep: sleep(Duration::from_millis(5)), initial_forward_travel: this.drivetrain.tracking.forward_travel(), start_time: now, - prev_time: now, + prev_time: None, linear_settled: false, angular_settled: false, } @@ -74,7 +74,7 @@ where return Poll::Pending; } - let dt = state.prev_time.elapsed(); + let dt = state.prev_time.map(|pt| pt.elapsed()); let forward_travel = this.drivetrain.tracking.forward_travel(); let position = this.drivetrain.tracking.position(); @@ -120,7 +120,7 @@ where ); state.sleep = sleep(Duration::from_millis(5)); - state.prev_time = Instant::now(); + state.prev_time = Some(Instant::now()); cx.waker().wake_by_ref(); Poll::Pending diff --git a/packages/evian-motion/src/seeking/boomerang.rs b/packages/evian-motion/src/seeking/boomerang.rs index 8c918c8..2817fbc 100644 --- a/packages/evian-motion/src/seeking/boomerang.rs +++ b/packages/evian-motion/src/seeking/boomerang.rs @@ -12,7 +12,7 @@ use evian_tracking::{TracksHeading, TracksPosition, TracksVelocity}; pub struct State { sleep: Sleep, - prev_time: Instant, + prev_time: Option, start_time: Instant, prev_position: Vec2, } @@ -60,7 +60,7 @@ where sleep: sleep(Duration::from_millis(5)), prev_position: this.drivetrain.tracking.position(), start_time: now, - prev_time: now, + prev_time: None, } }); @@ -68,7 +68,7 @@ where return Poll::Pending; } - let dt = state.prev_time.elapsed(); + let dt = state.prev_time.map(|pt| pt.elapsed()); let position = this.drivetrain.tracking.position(); let heading = this.drivetrain.tracking.heading(); @@ -106,13 +106,14 @@ where let linear_output = this.linear_controller.update(-linear_error, 0.0, dt) * angular_error.cos(); - drop(this - .drivetrain - .model - .drive_arcade(linear_output, angular_output)); + drop( + this.drivetrain + .model + .drive_arcade(linear_output, angular_output), + ); state.sleep = sleep(Duration::from_millis(5)); - state.prev_time = Instant::now(); + state.prev_time = Some(Instant::now()); state.prev_position = position; cx.waker().wake_by_ref(); diff --git a/packages/evian-motion/src/seeking/move_to_point.rs b/packages/evian-motion/src/seeking/move_to_point.rs index e85dcf2..f8ae335 100644 --- a/packages/evian-motion/src/seeking/move_to_point.rs +++ b/packages/evian-motion/src/seeking/move_to_point.rs @@ -13,7 +13,7 @@ use evian_tracking::{TracksHeading, TracksPosition, TracksVelocity}; pub(crate) struct State { sleep: Sleep, close: bool, - prev_time: Instant, + prev_time: Option, start_time: Instant, } @@ -58,7 +58,7 @@ where State { sleep: sleep(Duration::from_millis(5)), start_time: now, - prev_time: now, + prev_time: None, close: false, } }); @@ -67,7 +67,7 @@ where return Poll::Pending; } - let dt = state.prev_time.elapsed(); + let dt = state.prev_time.map(|pt| pt.elapsed()); let position = this.drivetrain.tracking.position(); let heading = this.drivetrain.tracking.heading(); @@ -113,7 +113,7 @@ where .drive_arcade(linear_output, angular_output); state.sleep = sleep(Duration::from_millis(5)); - state.prev_time = Instant::now(); + state.prev_time = Some(Instant::now()); cx.waker().wake_by_ref(); Poll::Pending