add lidar limit

pod-operation/src/state_machine.rs

@@ -9,6 +9,7 @@ use tracing::info;
 
 use crate::components::brakes::Brakes;
 use crate::components::high_voltage_system::HighVoltageSystem;
+use crate::components::lidar::Lidar;
 use crate::components::lim_temperature::LimTemperature;
 use crate::components::pressure_transducer::PressureTransducer;
 use crate::components::signal_light::SignalLight;
@@ -17,7 +18,7 @@ use crate::components::wheel_encoder::WheelEncoder;
 const TICK_INTERVAL: Duration = Duration::from_millis(10);
 const STOP_THRESHOLD: f32 = 37.0; // Meters
 const MIN_PRESSURE: f32 = 126.0; // PSI
-
+const END_OF_TRACK: f32 = 8.7; // Meters
 const LIM_TEMP_THRESHOLD: f32 = 71.0; //°C
 
 #[derive(Clone, Copy, Debug, PartialEq, Eq, enum_map::Enum)]
@@ -46,6 +47,7 @@ pub struct StateMachine {
 	lim_temperature_port: LimTemperature,
 	lim_temperature_starboard: LimTemperature,
 	high_voltage_system: HighVoltageSystem,
+	lidar: Lidar,
 }
 
 impl StateMachine {
@@ -102,6 +104,7 @@ impl StateMachine {
 				false, true,
 			)),
 			high_voltage_system: HighVoltageSystem::new(),
+			lidar: Lidar::new(),
 		}
 	}
 
@@ -161,33 +164,33 @@ impl StateMachine {
 
 	fn _enter_init(&mut self) {
 		info!("Entering Init state");
-		self.signal_light.disable();
+		// self.signal_light.disable();
 	}
 
 	fn _enter_load(&mut self) {
 		info!("Entering Load state");
-		self.brakes.disengage();
-		self.signal_light.disable();
+		// self.brakes.disengage();
+		// self.signal_light.disable();
 	}
 
 	fn _enter_running(&mut self) {
 		info!("Entering Running state");
-		self.high_voltage_system.enable(); // Enable high voltage system -- may move later
-		self.signal_light.enable();
-		self.brakes.disengage();
+		// self.high_voltage_system.enable(); // Enable high voltage system -- may move later
+		// self.signal_light.enable();
+		// self.brakes.disengage();
 	}
 
 	fn _enter_stopped(&mut self) {
 		info!("Entering Stopped state");
-		self.signal_light.disable();
-		self.brakes.engage();
+		// self.signal_light.disable();
+		// self.brakes.engage();
 	}
 
 	fn _enter_halted(&mut self) {
 		info!("Entering Halted state");
-		self.signal_light.disable();
-		self.brakes.engage();
-		self.high_voltage_system.disable();
+		// self.signal_light.disable();
+		// self.brakes.engage();
+		// self.high_voltage_system.disable();
 	}
 
 	fn _enter_faulted(&mut self) {
@@ -197,9 +200,9 @@ impl StateMachine {
 			.unwrap()
 			.emit("fault", "123")
 			.ok();
-		self.signal_light.disable();
-		self.brakes.engage();
-		self.high_voltage_system.disable();
+		// self.signal_light.disable();
+		// self.brakes.engage();
+		// self.high_voltage_system.disable();
 	}
 
 	/// Perform operations when the pod is loading
@@ -228,6 +231,10 @@ impl StateMachine {
 		{
 			return State::Faulted;
 		}
+		// Last 20% of the track, as indicated by braking
+		if self.lidar.read_distance() < END_OF_TRACK {
+			return State::Faulted;
+		}
 
 		State::Running
 	}