From 997540b2c3856e2c87f13d3fd089883f619f5133 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Feb 2022 11:18:15 -0700 Subject: [PATCH] gyro regression --- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 681fcb9..c456027 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -56,7 +56,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; - public Rotation2d rotTarget = new Rotation2d();; + public Rotation2d rotTarget = new Rotation2d(); private final Field2d m_field = new Field2d(); @@ -204,6 +204,15 @@ public class SwerveDrive extends SubsystemBase { return m_poseEstimator.getEstimatedPosition(); } + /** + * Gets the current gyro using regression formula. + * @return Rotation2d object holding current gyro in radians + */ + public Rotation2d getRegGyro() { + double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + return new Rotation2d(regCur * Math.PI / 180); + } + /** * Resets the odometry of the robot to the given pose. */ @@ -211,10 +220,10 @@ public class SwerveDrive extends SubsystemBase { m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); } - /** Updates the field relative position of the robot. */ - + /** Updates the field relative position of the robot. + */ public void updateOdometry() { - m_poseEstimator.update( m_gyro.getRotation2d(), + m_poseEstimator.update( getRegGyro(), modules[0].getState(), modules[1].getState(), modules[2].getState(),