Extract _should_stop decision

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.