From c56202654f6e59c5d5e57ef3864606ce4780dce4 Mon Sep 17 00:00:00 2001 From: Taesung Hwang Date: Thu, 30 May 2024 16:06:56 -0700 Subject: [PATCH] Add resting velocity and wheel diameter --- pod-operation/src/components/wheel_encoder.rs | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/pod-operation/src/components/wheel_encoder.rs b/pod-operation/src/components/wheel_encoder.rs index 0a3608be..d7eae377 100644 --- a/pod-operation/src/components/wheel_encoder.rs +++ b/pod-operation/src/components/wheel_encoder.rs @@ -4,8 +4,9 @@ use rppal::gpio::{Gpio, InputPin, Level}; use crate::utils::GpioPins; -// TODO: wheel diameter -const DELTA_D: f32 = 1.0 / 16.0; +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)] @@ -109,16 +110,19 @@ impl WheelEncoder { let dt = time.duration_since(self.last_time).as_secs_f32(); if inc != EncoderDiff::Stationary { - self.velocity = DELTA_D * f32::from(i8::from(inc)) / dt; + self.velocity = DISTANCE_PER_COUNT * f32::from(i8::from(inc)) / dt; self.last_time = time; } - // TODO: fix resting velocity + // 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; - Ok(f32::from(self.counter) * DELTA_D) + Ok(f32::from(self.counter) * DISTANCE_PER_COUNT) } pub fn get_velocity(&self) -> f32 {