From 84ab1b7b4077dfdc1c07cf3116b91008a3a12f26 Mon Sep 17 00:00:00 2001
From: ryescholin <46325761+ryescholin@users.noreply.github.com>
Date: Fri, 31 May 2024 00:10:37 -0700
Subject: [PATCH] Implement emergency stop in FSM using Lidar (#95)

* add lidar limit

* uncomment
---
 pod-operation/src/state_machine.rs | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/pod-operation/src/state_machine.rs b/pod-operation/src/state_machine.rs
index b1e93691..7093f070 100644
--- a/pod-operation/src/state_machine.rs
+++ b/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(),
 		}
 	}
 
@@ -229,6 +232,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
 	}