Do not reset gyro on pose estimator reset

frc868 · Mar 21, 2024 · 433da39 · 433da39
1 parent 968e812
commit 433da39
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -569,9 +569,11 @@ public void updatePoseEstimator() {
 
     @Override
     public void resetPoseEstimator(Pose2d pose) {
-        poseEstimator.resetPosition(getRotation(), getModulePositions(), pose);
+        poseEstimator.resetPosition(getRotation(), getModulePositions(),
+                new Pose2d(pose.getTranslation(), getRotation()));
         if (RobotBase.isSimulation())
-            simOdometry.resetPosition(getRotation(), getModulePositions(), pose);
+            simOdometry.resetPosition(getRotation(), getModulePositions(),
+                    new Pose2d(pose.getTranslation(), getRotation()));
     }
 
     @Override