advanced align to cage

Author: MichaelLesirge

src/main/java/frc/robot/RobotContainer.java

@@ -407,8 +407,8 @@ private void configureDriverControllerBindings(
         xbox.povRight(), ManualAlignCommands.alignToSourceRight(drive, input));
     configureAlignmentAuto.accept(
         xbox.povLeft(), ManualAlignCommands.alignToSourceLeft(drive, input));
-    configureAlignmentAuto.accept(xbox.povUp(), ManualAlignCommands.alignToCage(drive, input));
-    configureAlignmentAuto.accept(xbox.povDown(), ManualAlignCommands.alignToReef(drive, input));
+    configureAlignmentAuto.accept(xbox.povDown(), ManualAlignCommands.alignToCageAdv(drive, input));
+    configureAlignmentAuto.accept(xbox.povUp(), ManualAlignCommands.alignToReef(drive, input));
 
     if (includeAutoAlign) {
       // Align to reef

src/main/java/frc/robot/commands/ManualAlignCommands.java

@@ -4,21 +4,45 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.FieldConstants.Barge;
 import frc.robot.FieldConstants.CoralStation;
 import frc.robot.FieldConstants.Reef;
 import frc.robot.commands.controllers.JoystickInputController;
 import frc.robot.commands.controllers.SpeedLevelController.SpeedLevel;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.utility.AllianceFlipUtil;
+import java.util.List;
 import java.util.Optional;
+import java.util.stream.Stream;
 
 public class ManualAlignCommands {
+
   public static Command alignToCage(Drive drivetrain, JoystickInputController input) {
     return DriveCommands.joystickHeadingDrive(
         drivetrain,
         input::getTranslationMetersPerSecond,
-        () -> Optional.of(AllianceFlipUtil.apply(Rotation2d.kZero)),
+        () -> Optional.of(allianceRotation().plus(Rotation2d.k180deg)),
+        () -> SpeedLevel.NO_LEVEL,
+        () -> true);
+  }
+
+  public static Command alignToCageAdv(Drive drivetrain, JoystickInputController input) {
+    return DriveCommands.joystickHeadingDrive(
+        drivetrain,
+        input::getTranslationMetersPerSecond,
+        () -> {
+          Translation2d robotPose = drivetrain.getRobotPose().getTranslation();
+
+          List<Translation2d> cagePose =
+              Stream.of(Barge.farCage, Barge.middleCage, Barge.closeCage)
+                  .map(AllianceFlipUtil::apply)
+                  .toList();
+
+          return Optional.of(
+              robotPose.minus(robotPose.nearest(cagePose)).getAngle().plus(allianceRotation()));
+        },
         () -> SpeedLevel.NO_LEVEL,
         () -> true);
   }
@@ -49,22 +73,23 @@ public static Command alignToReef(Drive drivetrain, JoystickInputController inpu
           Translation2d robotPose = drivetrain.getRobotPose().getTranslation();
           Translation2d reefPose = AllianceFlipUtil.apply(Reef.center);
 
-          double dx = robotPose.getX() - reefPose.getX();
-          double dy = robotPose.getY() - reefPose.getY();
-
-          double angleRad = Math.atan2(dy, dx);
+          double angleRad = robotPose.minus(reefPose).getAngle().getRadians();
 
           for (Pose2d face : Reef.alignmentFaces) {
             double faceRad = face.getRotation().getRadians();
 
-            if (MathUtil.isNear(faceRad, angleRad, 0.5)) {
+            if (MathUtil.isNear(faceRad, angleRad, Units.degreesToRadians(25))) {
               angleRad = faceRad;
             }
           }
 
-          return Optional.of(Rotation2d.fromRadians(angleRad));
+          return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromRadians(angleRad)));
         },
         () -> SpeedLevel.NO_LEVEL,
         () -> true);
   }
+
+  private static Rotation2d allianceRotation() {
+    return AllianceFlipUtil.shouldFlip() ? Rotation2d.kZero : Rotation2d.k180deg;
+  }
 }