From a4361708dadfa1cb94512393bd51d7afcdb2f368 Mon Sep 17 00:00:00 2001 From: David Vo Date: Mon, 12 Feb 2024 18:43:58 +1100 Subject: [PATCH] chassis: Latency compensate drive encoder readings --- components/chassis.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/components/chassis.py b/components/chassis.py index 0e71db5c..f7493037 100644 --- a/components/chassis.py +++ b/components/chassis.py @@ -109,6 +109,8 @@ def __init__( else InvertedValue.COUNTER_CLOCKWISE_POSITIVE ) + # Note: we change the dimension of the drive encoder readings + # here from wheel rotations to metres. drive_gear_ratio_config = FeedbackConfigs().with_sensor_to_mechanism_ratio( 1 / self.DRIVE_MOTOR_REV_TO_METRES ) @@ -145,11 +147,14 @@ def get_rotation(self) -> Rotation2d: return Rotation2d(self.get_angle_integrated()) def get_speed(self) -> float: - # velocity is in rot/s, return in m/s - return self.drive.get_velocity().value + return self.drive.get_velocity().get_latency_compensated_value( + self.drive.get_acceleration() + ) def get_distance_traveled(self) -> float: - return self.drive.get_position().value + return self.drive.get_position().get_latency_compensated_value( + self.drive.get_velocity() + ) def set(self, desired_state: SwerveModuleState): if self.module_locked: