diff --git a/components/chassis.py b/components/chassis.py index e72413cc..46e19b12 100644 --- a/components/chassis.py +++ b/components/chassis.py @@ -138,7 +138,10 @@ def get_angle_absolute(self) -> float: def get_angle_integrated(self) -> float: """Gets steer angle from motor's integrated relative encoder""" - return self.steer.get_position().value * math.tau + rot = self.steer.get_position().get_latency_compensated_value( + self.steer.get_velocity() + ) + return rot * math.tau def get_rotation(self) -> Rotation2d: """Get the steer angle as a Rotation2d"""