From 26694ff7084c87fcba76e55c51fc6d1e21a1eafb Mon Sep 17 00:00:00 2001
From: Rick Fryar <rickf@mindless.com>
Date: Mon, 12 Feb 2024 21:29:51 -0600
Subject: [PATCH] Update SwerveSubsystem.java

Add zero the gyro with alliance method
---
 .../swervedrive/SwerveSubsystem.java          | 22 +++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index d107d6e6..07c7a7f2 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -390,6 +390,28 @@ public void zeroGyro()
     swerveDrive.zeroGyro();
   }
 
+  private boolean isRedAlliance() 
+  {
+    var alliance = DriverStation.getAlliance();
+    return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+  }
+
+  /**
+   * This will zero (calibrate) the robot to assume the current position is facing forward
+   * 
+   * If red alliance rotate the robot 180 after the drviebase zero command
+   */
+  public void zeroGyroWithAlliance() 
+  {
+    if (isRedAlliance()) {
+      zeroGyro();
+      //Set the pose 180 degrees
+      resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
+    } else {
+       zeroGyro();      
+    }
+  }  
+  
   /**
    * Sets the drive motors to brake/coast mode.
    *