From 0cc462f0dee04431297f663ed3de50810a1818ba Mon Sep 17 00:00:00 2001
From: MysticalApple <timon.cappellapimor@gmail.com>
Date: Fri, 5 Apr 2024 14:22:46 -0700
Subject: [PATCH] autoalign constructor fixes

---
 src/main/java/frc/robot/RobotContainer.java   | 15 +++++----
 .../robot/commands/swerve/AlignCommand.java   | 32 +++++++------------
 2 files changed, 20 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 97c2f324..0313fbb4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -187,11 +187,11 @@ public RobotContainer() {
         }
 
         try {
-        driverCamera = new UsbCamera("fisheye", 0);
-        driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30);
-        driverCamera.setExposureManual(40);
-        driverCameraServer = new MjpegServer("m1", 1181);
-        driverCameraServer.setSource(driverCamera);
+            driverCamera = new UsbCamera("fisheye", 0);
+            driverCamera.setVideoMode(PixelFormat.kMJPEG, 160, 120, 30);
+            driverCamera.setExposureManual(40);
+            driverCameraServer = new MjpegServer("m1", 1181);
+            driverCameraServer.setSource(driverCamera);
         } catch (Exception e) {
             System.out.print(e);
         }
@@ -273,7 +273,7 @@ private void configureBindings() {
         driveController.getAmpAlign().onTrue(new InstantCommand(
             () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
             ).andThen(new ParallelRaceGroup(
-                AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
+                AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem::isRedAlliance),
                 new ConditionalWaitCommand(
                     () -> !driveController.getAmpAlign().getAsBoolean()))
                     ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
@@ -282,7 +282,8 @@ private void configureBindings() {
         /* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its
          * climb hooks will end up directly above the center of the nearest chain. */
         driveController.getStageAlignButton().onTrue(
-            AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance())
+            AlignCommand.getStageAlignCommand(swerveSubsystem,
+                                              swerveSubsystem::getRobotPosition, fmsSubsystem::isRedAlliance)
                 .onlyWhile(driveController.getStageAlignButton())
         );
 
diff --git a/src/main/java/frc/robot/commands/swerve/AlignCommand.java b/src/main/java/frc/robot/commands/swerve/AlignCommand.java
index f6e2f888..cb9f9e84 100644
--- a/src/main/java/frc/robot/commands/swerve/AlignCommand.java
+++ b/src/main/java/frc/robot/commands/swerve/AlignCommand.java
@@ -14,6 +14,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.AutoAlignConstants;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.util.Pose2dSupplier;
+import java.util.function.BooleanSupplier;
 
 /** Contains functions to create auto-alignment commands. */
 public class AlignCommand {
@@ -45,32 +47,20 @@ public static Command getAlignCommand(Pose2d targetPose, SwerveSubsystem swerveS
      * Creates a PathPlanner align command using PathPlanner's pathfindToPose() and autoBuilder.
      *
      * @param swerveSubsystem The robot swerve subsystem to control
-     * @param isRed Whether or not we have a red alliance
+     * @param isRedSup True if the robot is on the red alliance, false if on blue.
      * @return Pathfinding Command that pathfinds and aligns the robot
      */
-    public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, Boolean isRed) {
+    public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, BooleanSupplier isRedSup) {
         
         Pose2d targetPose;
 
-        if (isRed) {
+        if (isRedSup.getAsBoolean()) {
             targetPose = AutoAlignConstants.RED_AMP_POSE;
         } else {
             targetPose = AutoAlignConstants.BLUE_AMP_POSE;
         }
-
-        Command command = AutoBuilder.pathfindToPose(
-            targetPose, 
-            new PathConstraints(
-            2.0, 2.0, 
-                    Units.degreesToRadians(720), Units.degreesToRadians(1080)
-                    ), 
-            0, 
-            0.0
-                );
-
-        command.addRequirements(swerveSubsystem);
         
-        return command;
+        return getAlignCommand(targetPose, swerveSubsystem);
     }
 
     /**
@@ -78,15 +68,17 @@ public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, Boolea
      * aligned directly above the chain. 
      *
      * @param swerveSubsystem The swerve drive to move the robot to its target.
-     * @param isRed True if the robot is on the red alliance, false if on the blue alliance.
+     * @param currentPoseSup A function that returns the current pose of the robot.
+     * @param isRedSup True if the robot is on the red alliance, false if on the blue alliance.
      * @return Pathfinding Command to bring the robot to its target position.
      */
-    public static Command getStageAlignCommand(SwerveSubsystem swerveSubsystem, Boolean isRed) {
-        Pose2d currentPose = swerveSubsystem.getRobotPosition();
+    public static Command getStageAlignCommand(SwerveSubsystem swerveSubsystem,
+                                               Pose2dSupplier currentPoseSup, BooleanSupplier isRedSup) {
+        Pose2d currentPose = currentPoseSup.getPose2d();
         Pose2d targetPose = new Pose2d();
         double distance = Double.MAX_VALUE;
 
-        Pose2d[] possibleTargets = isRed
+        Pose2d[] possibleTargets = isRedSup.getAsBoolean()
             ? new Pose2d[]{RED_STAGE_BACK_POSE, RED_STAGE_LEFT_POSE, RED_STAGE_RIGHT_POSE}
             : new Pose2d[]{BLUE_STAGE_BACK_POSE, BLUE_STAGE_LEFT_POSE, BLUE_STAGE_RIGHT_POSE};