From 26e5660ada22b1a987fe58d2af2f5e218e0be852 Mon Sep 17 00:00:00 2001
From: Taesung Hwang <taesungh@uci.edu>
Date: Sat, 1 Jun 2024 04:35:42 -0700
Subject: [PATCH] Extract `_should_stop` decision

---
 pod-operation/src/state_machine.rs | 32 ++++++++++++++++++------------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/pod-operation/src/state_machine.rs b/pod-operation/src/state_machine.rs
index cc2dee7..387e425 100644
--- a/pod-operation/src/state_machine.rs
+++ b/pod-operation/src/state_machine.rs
@@ -218,20 +218,10 @@ impl StateMachine {
 	fn _running_periodic(&mut self) -> State {
 		info!("Rolling Running state");
 
-		let encoder_value = self.wheel_encoder.measure().expect("wheel encoder faulted"); // Read the encoder value
+		let distance = self.wheel_encoder.measure().expect("wheel encoder faulted"); // Read the encoder value
+		let velocity = self.wheel_encoder.get_velocity();
 
-		// 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 {
+		if StateMachine::_should_stop(distance, velocity) {
 			return State::Stopped;
 		}
 
@@ -256,6 +246,22 @@ impl StateMachine {
 		State::Running
 	}
 
+	/// Consider two stopping conditions based on the pod's distance and velocity
+	/// at the next tickwhich is when the stopping will actually initiate
+	fn _should_stop(distance: f32, 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 distance + 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.