Extract wheel encoder to separate thread for faster sampling

pod-operation/src/components/wheel_encoder.rs

@@ -137,6 +137,10 @@ impl WheelEncoder {
 		self.velocity
 	}
 
+	pub fn get_distance(&self) -> f32 {
+		f32::from(self.counter) * DISTANCE_PER_COUNT
+	}
+
 	fn read_state(&self) -> EncoderState {
 		encode_state(self.pin_a.read(), self.pin_b.read())
 	}

pod-operation/src/state_machine.rs

@@ -17,11 +17,12 @@ use crate::components::pressure_transducer::PressureTransducer;
 use crate::components::signal_light::SignalLight;
 use crate::components::wheel_encoder::WheelEncoder;
 
-const TICK_INTERVAL: Duration = Duration::from_millis(10);
+const TICK_INTERVAL: Duration = Duration::from_millis(100);
 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
+const ENCODER_SAMPLE_INTERVAL: Duration = Duration::from_micros(100);
 
 #[derive(Clone, Copy, Debug, PartialEq, Eq, enum_map::Enum)]
 pub enum State {
@@ -43,13 +44,13 @@ pub struct StateMachine {
 	io: SocketIo,
 	brakes: Brakes,
 	signal_light: SignalLight,
-	wheel_encoder: WheelEncoder,
 	upstream_pressure_transducer: PressureTransducer,
 	downstream_pressure_transducer: PressureTransducer,
 	lim_temperature_port: LimTemperature,
 	lim_temperature_starboard: LimTemperature,
 	high_voltage_system: HighVoltageSystem,
 	lidar: Lidar,
+	wheel_encoder: std::sync::Arc<std::sync::Mutex<WheelEncoder>>,
 	gyro: Gyroscope,
 }
 
@@ -99,7 +100,6 @@ impl StateMachine {
 			io,
 			brakes: Brakes::new(),
 			signal_light: SignalLight::new(),
-			wheel_encoder: WheelEncoder::new(),
 			upstream_pressure_transducer: PressureTransducer::upstream(),
 			downstream_pressure_transducer: PressureTransducer::downstream(),
 			lim_temperature_port: LimTemperature::new(ads1x1x::SlaveAddr::Default),
@@ -109,18 +109,43 @@ impl StateMachine {
 			high_voltage_system: HighVoltageSystem::new(),
 			lidar: Lidar::new(),
 			gyro: Gyroscope::new(),
+			wheel_encoder: std::sync::Arc::new(std::sync::Mutex::new(WheelEncoder::new())),
 		}
 	}
 
 	pub async fn run(&mut self) {
 		let mut interval = tokio::time::interval(TICK_INTERVAL);
+		let encoder = self.wheel_encoder.clone();
+
+		tokio::spawn(Self::wheel_encoder_task(encoder));
 
 		loop {
 			self.tick().await;
 			interval.tick().await;
 		}
 	}
 
+	async fn wheel_encoder_task(wheel_encoder: std::sync::Arc<std::sync::Mutex<WheelEncoder>>) {
+		let mut interval = tokio::time::interval(ENCODER_SAMPLE_INTERVAL);
+
+		loop {
+			// Lock the mutex, measure, then immediately unlock
+			let _value = {
+				let mut encoder = wheel_encoder.lock().unwrap();
+				match encoder.measure() {
+					Ok(value) => value,
+					Err(e) => {
+						info!("Wheel encoder error: {:?}", e);
+						continue;
+					}
+				}
+			};
+			//info!("Wheel encoder value: {}", value);
+
+			interval.tick().await;
+		}
+	}
+
 	/// Tick the state machine by running the transition for the current state
 	/// and actions for when entering a new state
 	async fn tick(&mut self) {
@@ -146,8 +171,6 @@ impl StateMachine {
 	fn pod_periodic(&mut self) {
 		// Reading each value individually
 		let gyro_data = self.gyro.read_orientation();
-		let wheel_encoder_distance = self.wheel_encoder.measure().expect("wheel encoder faulted");
-		let wheel_encoder_velocity = self.wheel_encoder.get_velocity();
 		let downstream_pressure_data = self.downstream_pressure_transducer.read_pressure();
 		let upstream_pressure_data = self.upstream_pressure_transducer.read_pressure();
 		let lim_temp_port_data = self.lim_temperature_port.read_lim_temps();
@@ -156,7 +179,6 @@ impl StateMachine {
 		// Full JSON object
 		let full_json = json!({
 			"gyroscope": gyro_data,
-			"wheel_encoder": { "distance": wheel_encoder_distance, "velocity": wheel_encoder_velocity },
 			"downstream_pressure_transducer": downstream_pressure_data,
 			"upstream_pressure_transducer": upstream_pressure_data,
 			"lim_temperature_port": lim_temp_port_data,
@@ -226,21 +248,36 @@ impl StateMachine {
 	/// Perform operations when the pod is loading
 	fn _load_periodic(&mut self) -> State {
 		info!("Rolling Load state");
-
 		State::Load
 	}
 
 	/// Perform operations when the pod is running
 	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
-		if encoder_value > STOP_THRESHOLD {
+
+		let encoder_value = self.wheel_encoder.lock().unwrap();
+		let distance = encoder_value.get_distance();
+		let velocity: f32 = encoder_value.get_velocity();
+
+		let full_json = json!({
+			"distance": distance,
+			"velocity": velocity,
+		});
+
+		self.io
+			.of("/control-station")
+			.unwrap()
+			.emit("serverResponse", full_json)
+			.ok();
+
+		if distance > STOP_THRESHOLD {
 			return State::Stopped;
 		}
 
 		if self.downstream_pressure_transducer.read_pressure() < MIN_PRESSURE {
 			return State::Faulted;
 		}
+
 		let default_readings = self.lim_temperature_port.read_lim_temps();
 		let alternative_readings = self.lim_temperature_starboard.read_lim_temps();
 		if default_readings

pod-operation/src/utils/gpio.rs

@@ -4,8 +4,8 @@
 pub enum GpioPins {
 	_RESERVED_I2C_SDA = 2,
 	_RESERVED_I2C_SCL = 3,
-	WHEEL_ENCODER_A = 23,
-	WHEEL_ENCODER_B = 24,
+	WHEEL_ENCODER_A = 14,
+	WHEEL_ENCODER_B = 15,
 	CONTACTOR_RELAY = 20,
 	SIGNAL_LIGHT_RELAY = 21,
 	PNEUMATICS_RELAY = 26,