Changed sim logic (still not working)

Strflightmight09 · Strflightmight09 · commit 28b5981eb5cd · 2024-11-21T22:37:20.000+02:00
diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java
@@ -70,7 +70,7 @@ private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPo
     }
 
     private MirrorableRotation2d getTargetAngle() {
-        final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();
+        final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();
         return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false);
     }
 
diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java
@@ -45,7 +45,7 @@ public class CommandConstants {
                     () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()),
                     () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
             ),
-            RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())),
+            RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())),
             SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true),
             SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
                     () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java
@@ -75,6 +75,6 @@ else if (difference > tolerance)
     }
 
     private Pose2d getCurrentRobotPose() {
-        return RobotContainer.POSE_ESTIMATOR.getCurrentPose();
+        return RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();
     }
 }
diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java
@@ -71,7 +71,7 @@ private double calculateNoteZDifference(double t) {
     private void configureStartingStats() {
         startingTimeSeconds = Timer.getFPGATimestamp();
 
-        final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose();
+        final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();
         final Rotation2d currentRobotAngle = currentRobotPose.getRotation();
         final double startingTangentialVelocity = getStartingTangentialVelocity();
 
diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java
@@ -48,7 +48,7 @@ private static Command getAutonomousPrepareForAmpCommand() {
                 GeneralCommands.runWhen(
                         AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN)
                                 .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH)),
-                        () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation().getDistance(
+                        () -> RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation().getDistance(
                                 FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS
                 ),
                 ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND)
diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java
@@ -104,7 +104,7 @@ private static Optional<Rotation2d> calculateRotationOverride() {
         if (RobotContainer.INTAKE.hasNote() || !NOTE_DETECTION_CAMERA.hasTargets())
             return Optional.empty();
 
-        final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();
+        final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();
         final Rotation2d targetRotation = NOTE_DETECTION_CAMERA.getTrackedObjectYaw().minus(currentRotation);
         return Optional.of(targetRotation);
     }
diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java
@@ -86,7 +86,7 @@ public Translation3d getRobotFieldRelativeVelocity() {
      */
     private Translation2d predictFutureTranslation(double predictionTime) {
         final Translation2d fieldRelativeVelocity = getRobotFieldRelativeVelocity().toTranslation2d();
-        final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation();
+        final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation();
         return currentPose.plus(fieldRelativeVelocity.times(predictionTime));
     }
 
@@ -101,7 +101,7 @@ private Translation2d predictFutureTranslation(double predictionTime) {
      * @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState}
      */
     private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) {
-        final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation();
+        final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation();
         final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget);
         final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond);
         final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget);
diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java
@@ -43,7 +43,7 @@ protected SimulationObjectDetectionCameraIO(String hostname) {
 
     @Override
     protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) {
-        final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentPose());
+        final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose());
         if (closestObjectYaw == null) {
             inputs.hasTargets = false;
         } else {
@@ -63,7 +63,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) {
     private void updateHeldObjectPose() {
         if (heldObject.length == 0)
             return;
-        heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose());
+        heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose());
     }
 
     private void updateObjectEjection() {
@@ -79,7 +79,7 @@ private void updateObjectEjection() {
     private void updateObjectCollection() {
         if (heldObject.length == 1 || !isCollecting())
             return;
-        final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose();
+        final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();
         final Translation2d robotTranslation = robotPose.getTranslation();
         for (Translation2d objectPlacement : objectsOnField) {
             if (objectPlacement.getDistance(robotTranslation) <= PICKING_UP_TOLERANCE_METERS) {
diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java
@@ -60,7 +60,7 @@ public void update() {
 
         logCameraInfo();
         if (RobotHardwareStats.isSimulation())
-            AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose());
+            AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose());
     }
 
     public boolean hasNewResult() {
@@ -109,7 +109,7 @@ public double getDistanceToBestTagMeters() {
      * @return the robot's pose
      */
     private Pose2d calculateBestRobotPose() {
-        final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();
+        final Rotation2d gyroHeadingAtTimestamp =  RobotHardwareStats.isSimulation() ? RobotContainer.SWERVE.getHeading() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();
         return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp);
     }
 
diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java
@@ -24,13 +24,13 @@ public class AprilTagCameraConstants {
             SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600,
             SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200,
             SIMULATION_CAMERA_FPS = 60,
-            SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35,
-            SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5,
-            SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10;
+            SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 0,//35,
+            SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 0,//5,
+            SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 0;//10;
     private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70);
     private static final double
-            SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25,
-            SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08;
+            SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0,//0.25,
+            SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0;//0.08;
     public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties();
 
     static {
diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java
@@ -78,7 +78,7 @@ private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) {
      */
     private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) {
         final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation();
-        return new Rotation3d(0, cameraRotation.getX(), -cameraRotation.getY());
+        return new Rotation3d(0, -cameraRotation.getY(), -cameraRotation.getZ());
     }
 
     /**
diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
@@ -45,7 +45,7 @@ public void close() {
 
     public void periodic() {
         updateFromVision();
-        field.setRobotPose(getCurrentPose());
+        field.setRobotPose(getCurrentEstimatedPose());
     }
 
     /**
@@ -61,10 +61,18 @@ public void resetPose(Pose2d currentPose) {
     /**
      * @return the estimated pose of the robot, relative to the blue alliance's driver station right corner
      */
-    public Pose2d getCurrentPose() {
+    public Pose2d getCurrentEstimatedPose() {
         return poseEstimator6328.getEstimatedPose();
     }
 
+    /**
+     * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner
+     */
+
+    public Pose2d getCurrentOdometryPose() {
+        return poseEstimator6328.getOdometryPose();
+    }
+
     /**
      * Updates the pose estimator with the given SWERVE wheel positions and gyro rotations.
      * This function accepts an array of SWERVE wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz).
@@ -86,7 +94,7 @@ public void setGyroHeadingToBestSolvePNPHeading() {
         }
 
         final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading();
-        resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading));
+        resetPose(new Pose2d(getCurrentEstimatedPose().getTranslation(), bestRobotHeading));
     }
 
     private void logTargetPath() {
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -95,7 +95,7 @@ public ChassisSpeeds getSelfRelativeVelocity() {
     }
 
     public ChassisSpeeds getFieldRelativeVelocity() {
-        return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation());
+        return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation());
     }
 
     /**
@@ -111,16 +111,16 @@ public boolean atPose(MirrorablePose2d pose2d) {
 
     public boolean atXAxisPosition(double xAxisPosition) {
         final double currentXAxisVelocity = getFieldRelativeVelocity().vxMetersPerSecond;
-        return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getX(), xAxisPosition, currentXAxisVelocity);
+        return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getX(), xAxisPosition, currentXAxisVelocity);
     }
 
     public boolean atYAxisPosition(double yAxisPosition) {
         final double currentYAxisVelocity = getFieldRelativeVelocity().vyMetersPerSecond;
-        return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getY(), yAxisPosition, currentYAxisVelocity);
+        return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getY(), yAxisPosition, currentYAxisVelocity);
     }
 
     public boolean atAngle(MirrorableRotation2d angle) {
-        final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES;
+        final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES;
         final boolean isAngleStill = Math.abs(getSelfRelativeVelocity().omegaRadiansPerSecond) < SwerveConstants.ROTATION_VELOCITY_TOLERANCE;
         Logger.recordOutput("Swerve/AtTargetAngle/isStill", isAngleStill);
         Logger.recordOutput("Swerve/AtTargetAngle/atTargetAngle", atTargetAngle);
@@ -135,7 +135,7 @@ public double[] getDriveWheelPositionsRadians() {
     }
 
     public Rotation2d getDriveRelativeAngle() {
-        final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();
+        final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();
         return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle;
     }
 
@@ -163,7 +163,7 @@ void lockSwerve() {
      * @param targetPose the target pose, relative to the blue alliance driver station's right corner
      */
     void pidToPose(MirrorablePose2d targetPose) {
-        final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose();
+        final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();
         final Pose2d mirroredTargetPose = targetPose.get();
         final double xSpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), mirroredTargetPose.getX());
         final double ySpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), mirroredTargetPose.getY());
@@ -182,7 +182,7 @@ void initializeDrive(boolean closedLoop) {
     }
 
     void resetRotationController() {
-        SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getDegrees());
+        SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation().getDegrees());
     }
 
     void setClosedLoop(boolean closedLoop) {
@@ -303,7 +303,7 @@ private void updateHardware() {
 
     private void configurePathPlanner() {
         AutoBuilder.configureHolonomic(
-                RobotContainer.POSE_ESTIMATOR::getCurrentPose,
+                RobotContainer.POSE_ESTIMATOR::getCurrentEstimatedPose,
                 (pose) -> {
                 },
                 this::getSelfRelativeVelocity,
@@ -321,7 +321,7 @@ private boolean atTranslationPosition(double currentTranslationPosition, double
     }
 
     private double calculateProfiledAngleSpeedToTargetAngle(MirrorableRotation2d targetAngle) {
-        final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();
+        final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();
         return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), targetAngle.get().getDegrees()));
     }
 
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java
@@ -149,7 +149,7 @@ private static Command getCurrentDriveToPoseCommand(MirrorablePose2d targetPose,
 
     private static Command getPathfindToPoseCommand(MirrorablePose2d targetPose, PathConstraints pathConstraints) {
         final Pose2d targetMirroredPose = targetPose.get();
-        final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose();
+        final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();
         if (currentPose.getTranslation().getDistance(targetMirroredPose.getTranslation()) < 0.35)
             return createOnTheFlyPathCommand(targetPose, pathConstraints);
         return AutoBuilder.pathfindToPose(targetMirroredPose, pathConstraints);
@@ -163,7 +163,7 @@ private static Command getPIDToPoseCommand(MirrorablePose2d targetPose) {
 
     private static Command createOnTheFlyPathCommand(MirrorablePose2d targetPose, PathConstraints constraints) {
         List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
-                RobotContainer.POSE_ESTIMATOR.getCurrentPose(),
+                RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(),
                 targetPose.get()
         );
 

Original file line number	Diff line number	Diff line change
`@@ -70,7 +70,7 @@ private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPo`
`70`	`70`	`}`
`71`	`71`
`72`	`72`	`private MirrorableRotation2d getTargetAngle() {`
`73`		`- final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();`
	`73`	`+ final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();`
`74`	`74`	`return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false);`
`75`	`75`	`}`
`76`	`76`
Original file line number	Diff line number	Diff line change
`@@ -75,6 +75,6 @@ else if (difference > tolerance)`
`75`	`75`	`}`
`76`	`76`
`77`	`77`	`private Pose2d getCurrentRobotPose() {`
`78`		`- return RobotContainer.POSE_ESTIMATOR.getCurrentPose();`
	`78`	`+ return RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose();`
`79`	`79`	`}`
`80`	`80`	`}`
Original file line number	Diff line number	Diff line change
`@@ -104,7 +104,7 @@ private static Optional<Rotation2d> calculateRotationOverride() {`
`104`	`104`	`if (RobotContainer.INTAKE.hasNote() \|\| !NOTE_DETECTION_CAMERA.hasTargets())`
`105`	`105`	`return Optional.empty();`
`106`	`106`
`107`		`- final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation();`
	`107`	`+ final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation();`
`108`	`108`	`final Rotation2d targetRotation = NOTE_DETECTION_CAMERA.getTrackedObjectYaw().minus(currentRotation);`
`109`	`109`	`return Optional.of(targetRotation);`
`110`	`110`	`}`
Original file line number	Diff line number	Diff line change
`@@ -60,7 +60,7 @@ public void update() {`
`60`	`60`
`61`	`61`	`logCameraInfo();`
`62`	`62`	`if (RobotHardwareStats.isSimulation())`
`63`		`- AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose());`
	`63`	`+ AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose());`
`64`	`64`	`}`
`65`	`65`
`66`	`66`	`public boolean hasNewResult() {`
`@@ -109,7 +109,7 @@ public double getDistanceToBestTagMeters() {`
`109`	`109`	`* @return the robot's pose`
`110`	`110`	`*/`
`111`	`111`	`private Pose2d calculateBestRobotPose() {`
`112`		`- final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();`
	`112`	`+ final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.SWERVE.getHeading() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation();`
`113`	`113`	`return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp);`
`114`	`114`	`}`
`115`	`115`