From 1e7d224d5ee348909f68c692d9067e616f768547 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 22 Mar 2021 18:13:37 -0600 Subject: [PATCH] reversed odometry WIP --- src/main/java/frc4388/robot/subsystems/Drive.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe96ca2..71afc4e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -331,10 +331,20 @@ public class Drive extends SubsystemBase { m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks); m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); + updateOdometry(false); + } - m_odometry.update(Rotation2d.fromDegrees( getHeading()), + public void updateOdometry(boolean reversed) { + if (reversed) { + m_odometry.update(Rotation2d.fromDegrees( getHeading()-180), + -inchesToMeters(getDistanceInches(m_rightFrontMotor)), + inchesToMeters(getDistanceInches(m_leftFrontMotor))); + } + else { + m_odometry.update(Rotation2d.fromDegrees( getHeading()), inchesToMeters(getDistanceInches(m_leftFrontMotor)), -inchesToMeters(getDistanceInches(m_rightFrontMotor))); + } } /**