chk, forgot stuff from testing setup

Author: ryescholin

pod-operation/src/components/wheel_encoder.rs

@@ -7,6 +7,7 @@ use crate::utils::GpioPins;
 const WHEEL_DIAMETER: f32 = 0.0762; // meters
 const ENCODER_RESOLUTION: f32 = 16.0; // pulses per revolution
 const DISTANCE_PER_COUNT: f32 = WHEEL_DIAMETER * std::f32::consts::PI / ENCODER_RESOLUTION; // feet
+const BRAKING_DECELERATION: f32 = -15.14; // m/s^2
 
 #[derive(Clone, Copy, num_enum::FromPrimitive, num_enum::IntoPrimitive)]
 #[repr(i8)]
@@ -122,6 +123,10 @@ impl WheelEncoder {
 		self.velocity
 	}
 
+	pub fn get_braking_distance(&self) -> f32 {
+		-self.velocity.powi(2) / (2.0 * BRAKING_DECELERATION)
+	}
+
 	fn read_state(&self) -> EncoderState {
 		encode_state(self.pin_a.read(), self.pin_b.read())
 	}

pod-operation/src/state_machine.rs

@@ -218,13 +218,21 @@ impl StateMachine {
 		info!("Rolling Running state");
 
 		let encoder_value = self.wheel_encoder.measure().expect("wheel encoder faulted"); // Read the encoder value
+
+		let current_braking_distance = self.wheel_encoder.get_braking_distance();
+
+		// 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 encoder_value <= STOP_THRESHOLD {
+		if predicted_braking_distance <= BRAKING_THRESHOLD {
 			return State::Stopped;
 		}