Full Resolution Wheel Encoder

pod-operation/src/components/wheel_encoder.rs

@@ -1,70 +1,94 @@
-use rppal::gpio::{Gpio, InputPin, Level};
-use std::time::Instant;
+use tokio::time::Instant;
+use rppal::gpio::{Gpio, InputPin};
 
-const PIN_ENCODER_A: u8 = 1;
-const PIN_ENCODER_B: u8 = 2;
 
+// GPIO pins for the encoder
+const PIN_ENCODER_A: u8 = 23; //need to move
+const PIN_ENCODER_B: u8 = 24; //need to move
+
+// Constants for calculations
+const DELTA_D: f32 = 1.0 / 16.0;
+
+type EncoderState = u8;
+type EncoderDiff = i8;
+
+// Function to encode the state
+// 00, 01, 10, 11
+fn encode_state(a: bool, b: bool) -> EncoderState {
+	((a as u8) << 1) + (a as u8 ^ b as u8)
+}
+
+// Function to calculate the difference in states
+// -1 => moving backwards
+// 0 => no movement
+// 1 => moving forward
+// 2 => Undersampling
+fn state_difference(p: EncoderState, q: EncoderState) -> EncoderDiff {
+	((p as i8 - q as i8 + 1) % 4 - 1) as EncoderDiff
+}
+
+// Struct to represent the wheel encoder
 pub struct WheelEncoder {
-	counter: f32,
+	counter: i32,
+	last_time: Instant,
+	last_state: EncoderState,
 	pin_a: InputPin,
 	pin_b: InputPin,
-	a_last_read: Level,
-	b_last_read: Level,
-	last_distance: f32,
-	last_velocity_time: Instant,
-	velocity: f32,
+	faulted: bool,
 }
 
 impl WheelEncoder {
-	pub fn new() -> Self {
-		let gpio = Gpio::new().unwrap();
-		WheelEncoder {
-			counter: 0.0,
-			pin_a: gpio.get(PIN_ENCODER_A).unwrap().into_input(),
-			pin_b: gpio.get(PIN_ENCODER_B).unwrap().into_input(),
-			a_last_read: Level::High,
-			b_last_read: Level::Low,
-			last_distance: 0.0,
-			last_velocity_time: Instant::now(),
-			velocity: 0.0,
-		}
+	// Constructor to initialize the encoder
+	pub fn new() -> Result<Self, rppal::gpio::Error> {
+		let gpio = Gpio::new()?;
+		let pin_a = gpio.get(PIN_ENCODER_A)?.into_input();
+		let pin_b = gpio.get(PIN_ENCODER_B)?.into_input();
+
+		let initial_state = encode_state(pin_a.is_high(), pin_b.is_high());
+
+		Ok(WheelEncoder {
+			counter: 0,
+			last_time: Instant::now(),
+			last_state: initial_state,
+			pin_a,
+			pin_b,
+			faulted: false,
+		})
 	}
 
-	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;
-			}
+	// Method to measure speed and distance
+	pub fn measure(&mut self) -> Result<(f32, f32), &'static str> {
+		if self.faulted {
+			return Err("WheelEncoder is in Faulted state");
 		}
 
-		self.a_last_read = a_state;
-		self.b_last_read = b_state;
+		let current_time = Instant::now();
+		let state = self.read_state();
 
-		(self.counter / 16.0) * 3.0
-	}
+		let mut speed = 0.0;
+		let inc = state_difference(state, self.last_state);
+
+		if inc == 2 {
+			self.faulted = true;
+			return Err("Undersampling. Transition to Faulted state please.");
+		}
+
+		if inc != 0 {
+			let delta_t = current_time.duration_since(self.last_time).as_secs_f32();
+			speed = inc as f32 * DELTA_D / delta_t;
+			self.last_time = current_time;
+		}
+
+		self.last_state = state;
+		self.counter += inc as i32;
+
+		let distance = self.counter as f32 * DELTA_D;
 
-	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((speed, distance))
 	}
 
-	pub fn get_velocity(&self) -> f32 {
-		self.velocity
+	// Private method to read the state of the encoder
+	fn read_state(&self) -> EncoderState {
+		encode_state(self.pin_a.is_high(), self.pin_b.is_high())
 	}
 }

pod-operation/src/components/wheel_encoder_full_resolution.rs

@@ -0,0 +1,95 @@
+use rppal::gpio::{Gpio, InputPin};
+use std::thread;
+use std::time::{Duration, Instant};
+use tracing::{debug, error, info};
+
+// GPIO pins for the encoder
+const PIN_ENCODER_A: u8 = 14;
+const PIN_ENCODER_B: u8 = 15;
+
+// Constants for calculations
+const DELTA_D: f64 = 1.0 / 16.0;
+
+type EncoderState = u8;
+type EncoderDiff = i8;
+
+// Function to encode the state
+// 00, 01, 10, 11
+fn encode_state(a: bool, b: bool) -> EncoderState {
+    ((a as u8) << 1) + (a as u8 ^ b as u8)
+}
+
+// Function to calculate the difference in states
+// -1 => moving backwards
+// 0 => no movement
+// 1 => moving forward
+// 2 => Undersampling
+fn state_difference(p: EncoderState, q: EncoderState) -> EncoderDiff {
+    ((p as i8 - q as i8 + 1) % 4 - 1) as EncoderDiff
+}
+
+// Struct to represent the wheel encoder
+pub struct WheelEncoder {
+    counter: i32,
+    last_time: Instant,
+    last_state: EncoderState,
+    pin_a: InputPin,
+    pin_b: InputPin,
+    faulted: bool,
+}
+
+impl WheelEncoder {
+    // Constructor to initialize the encoder
+    pub fn new() -> Result<Self, rppal::gpio::Error> {
+        let gpio = Gpio::new()?;
+        let pin_a = gpio.get(PIN_ENCODER_A)?.into_input();
+        let pin_b = gpio.get(PIN_ENCODER_B)?.into_input();
+
+        let initial_state = encode_state(pin_a.is_high(), pin_b.is_high());
+
+        Ok(WheelEncoder {
+            counter: 0,
+            last_time: Instant::now(),
+            last_state: initial_state,
+            pin_a,
+            pin_b,
+            faulted: false,
+        })
+    }
+
+    // Method to measure speed and distance 
+    pub fn measure(&mut self) -> Result<(f64, f64), &'static str> {
+        if self.faulted {
+            return Err("WheelEncoder is in Faulted state");
+        }
+
+        let current_time = Instant::now();
+        let state = self.read_state();
+
+        let mut speed = 0.0;
+        let inc = state_difference(state, self.last_state);
+
+        if inc == 2 {
+            self.faulted = true;
+            return Err("Undersampling. Transition to Faulted state please.");
+        }
+
+        if inc != 0 {
+            let delta_t = current_time.duration_since(self.last_time).as_secs_f64();
+            speed = inc as f64 * DELTA_D / delta_t;
+            self.last_time = current_time;
+        }
+
+        self.last_state = state;
+        self.counter += inc as i32;
+
+        let distance = self.counter as f64 * DELTA_D;
+
+        Ok((speed, distance))
+    }
+
+    // Private method to read the state of the encoder
+    fn read_state(&self) -> EncoderState {
+        encode_state(self.pin_a.is_high(), self.pin_b.is_high())
+    }
+}

pod-operation/src/demo.rs

@@ -8,6 +8,7 @@ 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;
 
@@ -66,11 +67,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()
-		);
+		let (speed, distance) = wheel_encoder
+			.measure()
+			.expect("Failed to measure speed and distance");
+		println!("Speed: {:.2} m/s, Distance: {:.2} m", speed, distance);
 		tokio::time::sleep(std::time::Duration::new(1, 0)).await;
 	}
 }

pod-operation/src/main.rs

@@ -36,7 +36,7 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
 	let ads1015 = LimTemperature::new(ads1x1x::SlaveAddr::Default);
 	tokio::spawn(demo::read_ads1015(ads1015));
 
-	let wheel_encoder = WheelEncoder::new();
+	let wheel_encoder = WheelEncoder::new().expect("Failed to initialize WheelEncoder");
 	tokio::spawn(demo::read_wheel_encoder(wheel_encoder));
 
 	let gyro: Gyroscope = Gyroscope::new();

pod-operation/src/state_machine.rs

@@ -84,7 +84,7 @@ impl StateMachine {
 			io,
 			brakes: Brakes::new(),
 			signal_light: SignalLight::new(),
-			wheel_encoder: WheelEncoder::new(),
+			wheel_encoder: WheelEncoder::new().expect("Failed to initialize WheelEncoder"),
 			//upstream_pressure_transducer: PressureTransducer::upstream(),
 			downstream_pressure_transducer: PressureTransducer::downstream(),
 		}
@@ -184,8 +184,8 @@ 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 {
+		let (_, distance) = self.wheel_encoder.measure().unwrap_or((0.0, 0.0));
+		if distance > STOP_THRESHOLD {
 			return State::Stopped;
 		}
 		if self.downstream_pressure_transducer.read_pressure() < MIN_PRESSURE {