diff --git a/pod-operation/src/state_machine.rs b/pod-operation/src/state_machine.rs index cc2dee7..a89fe62 100644 --- a/pod-operation/src/state_machine.rs +++ b/pod-operation/src/state_machine.rs @@ -220,18 +220,8 @@ impl StateMachine { let encoder_value = self.wheel_encoder.measure().expect("wheel encoder faulted"); // Read the encoder value - // Predict next tick's braking distance - let current_velocity = self.wheel_encoder.get_velocity(); - let predicted_velocity = - current_velocity + BRAKING_DECELERATION * TICK_INTERVAL.as_secs_f32(); - let predicted_braking_distance = -predicted_velocity.powi(2) / (2.0 * BRAKING_DECELERATION); - - // Check if the predicted braking distance requires stopping - if encoder_value + current_velocity * TICK_INTERVAL.as_secs_f32() >= STOP_THRESHOLD { - return State::Stopped; - } - - if predicted_braking_distance <= BRAKING_THRESHOLD { + let velocity = self.wheel_encoder.get_velocity(); + if StateMachine::_should_stop(velocity) { return State::Stopped; } @@ -256,6 +246,22 @@ impl StateMachine { State::Running } + /// Consider two stopping conditions based on the pod's velocity at the next tick + /// which is when the stopping will actually initiate + fn _should_stop(velocity: f32) -> bool { + // Predict next tick's braking distance + let predicted_velocity = + velocity + BRAKING_DECELERATION * TICK_INTERVAL.as_secs_f32(); + let predicted_braking_distance = -predicted_velocity.powi(2) / (2.0 * BRAKING_DECELERATION); + + // Check if the predicted braking distance requires stopping + if encoder_value + velocity * TICK_INTERVAL.as_secs_f32() >= STOP_THRESHOLD { + return true; + } + + predicted_braking_distance <= BRAKING_THRESHOLD + } + // To avoid conflicts with the state-transition model, // each of these event handlers must wait for an ongoing transition to complete // by awaiting the mutex lock to be acquired.