autoalign constructor fixes

grt192 · Apr 6, 2024 · 82dfd60 · 82dfd60
1 parent 1b82b8a
commit 82dfd60
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 27 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -189,11 +189,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);
         }
@@ -298,7 +298,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/auton/AutoAmpSequence.java b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java
@@ -30,7 +30,7 @@ public AutoAmpSequence(FieldManagementSubsystem fms,
             /* Prepares the mechanisms for amping while path-finding the robot to its amping position. */
             Commands.parallel(
                 new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem),
-                AlignCommand.getAmpAlignCommand(swerveSubsystem, fms.isRedAlliance())
+                AlignCommand.getAmpAlignCommand(swerveSubsystem, fms::isRedAlliance)
             ),
 
             /* Deposits the note into the amp. */

diff --git 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,48 +47,38 @@ 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);
     }
 
     /**
      * Generates a holonomic path from the robot to the nearest stage face. The robot should end up with its climb hooks
      * 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};