From 6e31230adc345ce50ef49691c2aaa2cb29423b5a Mon Sep 17 00:00:00 2001 From: Jeff Hutchison Date: Sat, 24 Apr 2021 23:07:04 -0400 Subject: [PATCH] [examples] Fix odometry update in SwerveControllerCommand example (#3310) The Drive Subsystem was supplying an incorrectly constructed Rotation2d to the odometry update method. Rotation2d constructor was being called with heading in degrees, not radians as required. --- .../swervecontrollercommand/subsystems/DriveSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 11d7f2cb73..e65ea481f1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; @@ -68,7 +67,7 @@ public class DriveSubsystem extends SubsystemBase { public void periodic() { // Update the odometry in the periodic block m_odometry.update( - new Rotation2d(getHeading()), + m_gyro.getRotation2d(), m_frontLeft.getState(), m_rearLeft.getState(), m_frontRight.getState(),