Merge branch 'main' of https://github.com/UCI-HyperXite/hx9-control-s…

…ystem into fsm_out

pod-operation/Cargo.toml

@@ -17,11 +17,11 @@ enum-map = "2.7.3"
 once_cell = "1.19.0"
 num_enum = "0.7.2"
 
-rppal = { version = "0.17.1", features = ["hal"] }
+rppal = { version = "0.18.0", features = ["hal"] }
 ina219 = "0.1.0"
 ads1x1x = "0.2.2"
 nb = "1.1.0"
 mpu6050 = "0.1.6"
 lidar_lite_v3 = "0.1.0"
 i2cdev = "0.3.1"
-byteorder = "1.4.3"
+byteorder = "1.4.3"

pod-operation/src/components/wheel_encoder.rs

@@ -1,76 +1,166 @@
-use std::time::Instant;
+use std::{ops::Sub, time::Instant};
 
 use rppal::gpio::{Gpio, InputPin, Level};
 
 use crate::utils::GpioPins;
 
+const WHEEL_DIAMETER: f32 = 0.25; // feet
+const ENCODER_RESOLUTION: f32 = 16.0; // pulses per revolution
+const DISTANCE_PER_COUNT: f32 = WHEEL_DIAMETER * std::f32::consts::PI / ENCODER_RESOLUTION; // feet
+
+#[derive(Clone, Copy, num_enum::FromPrimitive, num_enum::IntoPrimitive)]
+#[repr(i8)]
+enum EncoderState {
+	A = 0b00,
+	B = 0b01,
+	C = 0b11,
+	D = 0b10,
+	#[num_enum(catch_all)]
+	Unknown(i8) = -1,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq, Eq, num_enum::FromPrimitive, num_enum::IntoPrimitive)]
+#[repr(i8)]
+enum EncoderDiff {
+	Backwards = -1,
+	Stationary = 0,
+	Forwards = 1,
+	Undersampling = 2,
+	#[num_enum(catch_all)]
+	Unknown(i8),
+}
+
+impl From<EncoderDiff> for i16 {
+	fn from(value: EncoderDiff) -> Self {
+		i16::from(i8::from(value))
+	}
+}
+
+impl From<EncoderDiff> for f32 {
+	fn from(value: EncoderDiff) -> Self {
+		f32::from(i8::from(value))
+	}
+}
+
+impl Sub for EncoderState {
+	type Output = EncoderDiff;
+
+	fn sub(self, other: Self) -> Self::Output {
+		let diff = (i8::from(self) - i8::from(other) + 5) % 4 - 1;
+		EncoderDiff::from(diff)
+	}
+}
+
+/// Encode wheel encoder state as gray code 00, 01, 11, 10
+fn encode_state(a: Level, b: Level) -> EncoderState {
+	let state = ((a as i8) << 1) + (a as i8 ^ b as i8);
+	EncoderState::from(state)
+}
+
 pub struct WheelEncoder {
-	counter: f32,
+	counter: i16,
 	pin_a: InputPin,
 	pin_b: InputPin,
-	a_last_read: Level,
-	b_last_read: Level,
-	last_distance: f32,
-	last_velocity_time: Instant,
+	last_state: EncoderState,
+	last_time: Instant,
 	velocity: f32,
 }
 
 impl WheelEncoder {
 	pub fn new() -> Self {
 		let gpio = Gpio::new().unwrap();
+		let pin_a = gpio
+			.get(GpioPins::WHEEL_ENCODER_A.into())
+			.unwrap()
+			.into_input();
+		let pin_b = gpio
+			.get(GpioPins::WHEEL_ENCODER_B.into())
+			.unwrap()
+			.into_input();
+
+		let initial_state = encode_state(pin_a.read(), pin_b.read());
+
 		WheelEncoder {
-			counter: 0.0,
-			pin_a: gpio
-				.get(GpioPins::WHEEL_ENCODER_A.into())
-				.unwrap()
-				.into_input(),
-			pin_b: gpio
-				.get(GpioPins::WHEEL_ENCODER_B.into())
-				.unwrap()
-				.into_input(),
-			a_last_read: Level::High,
-			b_last_read: Level::Low,
-			last_distance: 0.0,
-			last_velocity_time: Instant::now(),
+			counter: 0,
+			last_time: Instant::now(),
+			last_state: initial_state,
+			pin_a,
+			pin_b,
 			velocity: 0.0,
 		}
 	}
 
-	pub fn read(&mut self) -> f32 {
-		let a_state = self.pin_a.read();
-		let b_state = self.pin_b.read();
-
-		if (a_state != self.a_last_read || b_state != self.b_last_read) && b_state != a_state {
-			self.counter += 1.0;
-
-			let current_time = Instant::now();
-			let distance = (self.counter / 16.0) * 3.0;
-			let velocity_elapsed = current_time
-				.duration_since(self.last_velocity_time)
-				.as_secs_f32();
-
-			if velocity_elapsed >= 0.1 {
-				let distance_delta = distance - self.last_distance;
-				self.velocity = distance_delta / velocity_elapsed;
-				self.last_velocity_time = current_time;
-				self.last_distance = distance;
-			}
+	pub fn measure(&mut self) -> Result<f32, &str> {
+		let state = self.read_state();
+
+		let inc = state - self.last_state;
+
+		if inc == EncoderDiff::Undersampling {
+			return Err("Wheel encoder faulted");
 		}
 
-		self.a_last_read = a_state;
-		self.b_last_read = b_state;
+		let time = Instant::now();
+		let dt = time.duration_since(self.last_time).as_secs_f32();
 
-		(self.counter / 16.0) * 3.0
-	}
+		if inc != EncoderDiff::Stationary {
+			self.velocity = DISTANCE_PER_COUNT * f32::from(i8::from(inc)) / dt;
+			self.last_time = time;
+		}
+
+		// When exceeding expected time to next increment, decrease velocity
+		if self.velocity * dt > DISTANCE_PER_COUNT {
+			self.velocity = DISTANCE_PER_COUNT * self.velocity.signum() / dt;
+		}
+
+		self.counter += i16::from(inc);
+		self.last_state = state;
 
-	pub fn _reset(&mut self) {
-		self.counter = 0.0;
-		self.last_distance = 0.0;
-		self.velocity = 0.0;
-		self.last_velocity_time = Instant::now();
+		Ok(f32::from(self.counter) * DISTANCE_PER_COUNT)
 	}
 
 	pub fn get_velocity(&self) -> f32 {
 		self.velocity
 	}
+
+	fn read_state(&self) -> EncoderState {
+		encode_state(self.pin_a.read(), self.pin_b.read())
+	}
+}
+
+#[cfg(test)]
+mod tests {
+	use super::*;
+
+	#[test]
+	fn encoder_diff_stationary() {
+		let state_1 = encode_state(Level::Low, Level::High);
+		let state_2 = encode_state(Level::Low, Level::High);
+		assert_eq!(state_2 - state_1, EncoderDiff::Stationary);
+	}
+
+	#[test]
+	fn encoder_diff_forwards() {
+		let state_1 = encode_state(Level::High, Level::Low);
+		let state_2 = encode_state(Level::Low, Level::Low);
+		let diff = state_2 - state_1;
+		assert_eq!(diff, EncoderDiff::Forwards);
+		assert_eq!(i8::from(diff), 1);
+	}
+
+	#[test]
+	fn encoder_diff_backwards() {
+		let state_1 = encode_state(Level::High, Level::Low);
+		let state_2 = encode_state(Level::High, Level::High);
+		let diff = state_2 - state_1;
+		assert_eq!(diff, EncoderDiff::Backwards);
+		assert_eq!(i8::from(diff), -1);
+		assert_eq!(f32::from(diff), -1.0);
+	}
+
+	#[test]
+	fn encoder_diff_undersampling() {
+		let state_1 = encode_state(Level::High, Level::Low);
+		let state_2 = encode_state(Level::Low, Level::High);
+		assert_eq!(state_2 - state_1, EncoderDiff::Undersampling);
+	}
 }

pod-operation/src/demo.rs

@@ -10,6 +10,7 @@ use crate::components::lim_temperature::LimTemperature;
 use crate::components::pressure_transducer::PressureTransducer;
 use crate::components::signal_light::SignalLight;
 use crate::components::wheel_encoder::WheelEncoder;
+
 pub async fn blink(mut signal_light: SignalLight) {
 	let mut i = 0;
 
@@ -82,12 +83,10 @@ pub async fn read_gyroscope(mut gyroscope: Gyroscope) {
 pub async fn read_wheel_encoder(mut wheel_encoder: WheelEncoder) {
 	info!("Starting wheel encoder demo.");
 	loop {
-		println!(
-			"{:?}{:?}",
-			wheel_encoder.read(),
-			wheel_encoder.get_velocity()
-		);
-		tokio::time::sleep(std::time::Duration::new(1, 0)).await;
+		let count = wheel_encoder.measure().expect("faulted");
+		let velocity = wheel_encoder.get_velocity();
+		println!("{:?} {:?}", count, velocity);
+		tokio::time::sleep(std::time::Duration::from_millis(1)).await;
 	}
 }
 

pod-operation/src/state_machine.rs

@@ -215,24 +215,21 @@ impl StateMachine {
 	/// Perform operations when the pod is running
 	fn _running_periodic(&mut self) -> State {
 		info!("Rolling Running state");
-		// let encoder_value = self.wheel_encoder.read(); // Read the encoder value
-		// if encoder_value > 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
-		// 	.iter()
-		// 	.chain(alternative_readings.iter())
-		// 	.any(|&reading| reading > LIM_TEMP_THRESHOLD)
-		// {
-		// 	return State::Faulted;
-		// }
-
-		if self.lidar.read_distance() < 4 {
+		let encoder_value = self.wheel_encoder.measure().expect("wheel encoder faulted"); // Read the encoder value
+		if encoder_value > 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
+			.iter()
+			.chain(alternative_readings.iter())
+			.any(|&reading| reading > LIM_TEMP_THRESHOLD)
+		{
 			return State::Faulted;
 		}