diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index d107d6e6..034aede1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/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. *