fix: Use driver-relative heading for driving.

Author: haydenheroux

Diff for: src/main/java/frc/robot/odometry/Odometry.java

@@ -47,7 +47,7 @@ public class Odometry extends Subsystem {
   private final Field2d field;
 
   /** List of functions to be called when pose is manually updated. */
-  private final List<Consumer<Pose2d>> poseUpdateConsumers;
+  private final List<Consumer<Rotation2d>> yawUpdateConsumers;
 
   /** Creates a new instance of the odometry subsystem. */
   private Odometry() {
@@ -73,7 +73,7 @@ private Odometry() {
 
     field = new Field2d();
 
-    poseUpdateConsumers = new ArrayList<Consumer<Pose2d>>();
+    yawUpdateConsumers= new ArrayList<Consumer<Rotation2d>>();
   }
 
   /**
@@ -110,7 +110,8 @@ public void addToShuffleboard(ShuffleboardTab tab) {
 
     position.addDouble("X (m)", () -> getPosition().getX());
     position.addDouble("Y (m)", () -> getPosition().getY());
-    position.addDouble("Rotation (deg)", () -> getPosition().getRotation().getDegrees());
+    position.addDouble("Field Rotation (deg)", () -> getFieldRelativeHeading().getDegrees());
+    position.addDouble("Driver Rotation (deg)", () -> getDriverRelativeHeading().getDegrees());
 
     ShuffleboardLayout velocity = Telemetry.addColumn(tab, "Velocity");
 
@@ -150,13 +151,7 @@ public Rotation2d getFieldRelativeHeading() {
    *     alliance.
    */
   public Rotation2d getDriverRelativeHeading() {
-    Rotation2d fieldRelativeHeading = getFieldRelativeHeading();
-
-    if (AllianceFlipHelper.shouldFlip()) {
-      return fieldRelativeHeading.plus(Rotation2d.fromDegrees(180));
-    }
-
-    return fieldRelativeHeading;
+    return Rotation2d.fromRotations(gyroscopeValues.yawRotations);
   }
 
   /**
@@ -187,25 +182,21 @@ public void setRotation(Rotation2d rotation) {
    *
    * @param consumer consumer for when pose is manually updated.
    */
-  public void onPoseUpdate(Consumer<Pose2d> consumer) {
-    poseUpdateConsumers.add(consumer);
+  public void onYawUpdate(Consumer<Rotation2d> consumer) {
+    yawUpdateConsumers.add(consumer);
   }
 
   /**
-   * Tares the rotation of the robot.
+   * Zeroes the driver-relative rotation of the robot.
    *
-   * @return a command that zeroes the rotation of the robot.
+   * @return a command that zeroes the driver-relative rotation of the robot.
    */
   public Command tare() {
     return Commands.runOnce(
         () -> {
-          if (AllianceFlipHelper.shouldFlip()) {
-            setRotation(Rotation2d.fromDegrees(180));
-          } else {
-            setRotation(Rotation2d.fromDegrees(0));
-          }
+          gyroscope.setYaw(0.0);
 
-          poseUpdateConsumers.forEach(consumer -> consumer.accept(getPosition()));
+          yawUpdateConsumers.forEach(consumer -> consumer.accept(Rotation2d.fromDegrees(0)));
         });
   }
 

Diff for: src/main/java/frc/robot/swerve/DriveCommand.java

@@ -56,7 +56,7 @@ public DriveCommand(CommandXboxController driverController) {
 
     headingSnapper = SnapRotation.to(Rotation2d.fromDegrees(90));
 
-    odometry.onPoseUpdate(newPose -> resetHeadingGoal());
+    odometry.onYawUpdate(newPose -> resetHeadingGoal());
   }
 
   @Override
@@ -78,7 +78,7 @@ public void execute() {
     }
 
     if (request.isSnapping()) {
-      setPositionHeadingGoal(headingSnapper.snap(request.fieldHeading()));
+      setPositionHeadingGoal(headingSnapper.snap(request.driverHeading()));
     }
 
     Rotation2d omega;
@@ -152,7 +152,7 @@ private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) {
    * @return the reference heading to use with the heading motion profile.
    */
   private Rotation2d getReferenceHeading() {
-    return odometry.getFieldRelativeHeading();
+    return odometry.getDriverRelativeHeading();
   }
 
   /**

Diff for: src/main/java/frc/robot/swerve/DriveRequest.java

@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.lib.AllianceFlipHelper;
 
 public record DriveRequest(
     DriveRequest.TranslationMode translationMode,
@@ -103,14 +102,4 @@ public Rotation2d omega() {
   public Rotation2d driverHeading() {
     return rotation().getAngle();
   }
-
-  public Rotation2d fieldHeading() {
-    Rotation2d heading = driverHeading();
-
-    if (AllianceFlipHelper.shouldFlip()) {
-      heading = heading.plus(Rotation2d.fromDegrees(180));
-    }
-
-    return heading;
-  }
 }