Merge pull request #175 from rickfmn/patch-2

Update SwerveSubsystem.java

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -390,6 +390,32 @@ public void zeroGyro()
     swerveDrive.zeroGyro();
   }
 
+  /**
+  * Checks if the alliance is red, defaults to false if alliance isn't available.
+  * @return true if the red alliance, false if blue. Defaults to false if none is available.
+  */
+  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.
    *