Merge branch 'stage-align'

src/main/java/frc/robot/Constants.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -43,7 +44,7 @@ public static class ElevatorConstants {
         public static final double EXTENSION_D = 0;
         public static final double EXTENSION_TOLERANCE = 0.008;
 
-        public static final double POSITION_CONVERSION_FACTOR = 1 /26.357; // Units.inchesToMeters(30.)/63.5;
+        public static final double POSITION_CONVERSION_FACTOR = 1 / 26.357; // Units.inchesToMeters(30.)/63.5;
         public static final double VELOCITY_CONVERSION_FACTOR = 1;
 
         public static final double DOWN_POWER = -0.001; //the motor power to make the elevator move down slowly
@@ -184,6 +185,44 @@ public static class AutoAlignConstants {
         public static final Pose2d RED_AMP_POSE = new Pose2d(Units.inchesToMeters(578.77),
                 Units.inchesToMeters(323.00 - robotRadius), Rotation2d.fromDegrees(90));
 
+        /* Stage poses such that the climb hooks are directly above the chains. Poses are defined in terms of the
+         * corresponding apriltag's pose transformed by a constant distance to the chain. "Stage Left", in this case,
+         * refers to the left third of the stage when facing the stage from the driver station. We are aware that this
+         * naming scheme differs from the theatre convention. */
+        private static final Transform2d STAGE_TAG_TO_ROBOT = new Transform2d(Units.inchesToMeters(17.0),
+                                                                              Units.inchesToMeters(0),
+                                                                              Rotation2d.fromDegrees(180));
+
+        public static final Pose2d BLUE_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(209.48),
+                                                                    Units.inchesToMeters(161.62),
+                                                                    Rotation2d.fromDegrees(0))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
+        public static final Pose2d BLUE_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(182.73),
+                                                                    Units.inchesToMeters(177.10),
+                                                                    Rotation2d.fromDegrees(120))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
+        public static final Pose2d BLUE_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(182.73),
+                                                                    Units.inchesToMeters(146.19),
+                                                                    Rotation2d.fromDegrees(240))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
+        public static final Pose2d RED_STAGE_BACK_POSE = new Pose2d(Units.inchesToMeters(441.74),
+                                                                    Units.inchesToMeters(161.62),
+                                                                    Rotation2d.fromDegrees(180))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
+        public static final Pose2d RED_STAGE_LEFT_POSE = new Pose2d(Units.inchesToMeters(468.69),
+                                                                    Units.inchesToMeters(146.19),
+                                                                    Rotation2d.fromDegrees(300))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
+        public static final Pose2d RED_STAGE_RIGHT_POSE = new Pose2d(Units.inchesToMeters(468.69),
+                                                                    Units.inchesToMeters(177.10),
+                                                                    Rotation2d.fromDegrees(60))
+                                                                    .transformBy(STAGE_TAG_TO_ROBOT);
+
     }
 
     /** Constants for auton. */

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

@@ -9,7 +9,6 @@
 import static frc.robot.Constants.VisionConstants.NOTE_CAMERA;
 
 import java.util.EnumSet;
-import java.util.function.BooleanSupplier;
 
 import com.choreo.lib.ChoreoTrajectory;
 import com.fasterxml.jackson.databind.util.Named;
@@ -224,11 +223,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);
         }
@@ -349,14 +348,14 @@ private void configureBindings() {
         /* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the
          * amp and deposit its note. Releasing the button will stop the robot (and the path finding). */
         // TODO: Bring back LED integration.
-        driveController.getAutoAmp().onTrue(
-            new AutoAmpSequence(fmsSubsystem,
-                                swerveSubsystem, 
-                                elevatorSubsystem,
-                                intakePivotSubsystem,
-                                intakeRollerSubsystem)
-
-                .onlyWhile(driveController.getAutoAmp())
+        driveController.getAutoAmp().whileTrue(
+            Commands.deferredProxy(() -> new AutoAmpSequence(
+                fmsSubsystem,
+                swerveSubsystem, 
+                elevatorSubsystem,
+                intakePivotSubsystem,
+                intakeRollerSubsystem
+            ))
         );
 
         // DEPRECATED AMP ALIGN
@@ -366,9 +365,22 @@ private void configureBindings() {
         //         AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
         //         new ConditionalWaitCommand(
         //             () -> !driveController.getAmpAlign().getAsBoolean()))
-        //      ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
+        //             ).andThen(
+        //                 new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))
+        //             )
         // );
 
+        /* 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().whileTrue(
+            Commands.deferredProxy(() -> AlignCommand.getStageAlignCommand(
+                swerveSubsystem,
+                swerveSubsystem::getRobotPosition,
+                fmsSubsystem::isRedAlliance
+            ))
+        );
+
+
         /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
         driveController.getNoteAlign().onTrue(
             new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,

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. */

src/main/java/frc/robot/commands/swerve/AlignCommand.java

@@ -1,12 +1,21 @@
 package frc.robot.commands.swerve;
 
+import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_BACK_POSE;
+import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_LEFT_POSE;
+import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_RIGHT_POSE;
+import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_BACK_POSE;
+import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_LEFT_POSE;
+import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_RIGHT_POSE;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
 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 {
@@ -38,31 +47,49 @@ 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;
         }
+
+        return getAlignCommand(targetPose, swerveSubsystem);
+    }
 
-        Command command = AutoBuilder.pathfindToPose(
-            targetPose, 
-            new PathConstraints(
-            2.0, 2.0, 
-                    Units.degreesToRadians(720), Units.degreesToRadians(1080)
-                    ), 
-            0, 
-            0.0
-                );
+    /**
+     * 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 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,
+                                               Pose2dSupplier currentPoseSup, BooleanSupplier isRedSup) {
+        Pose2d currentPose = currentPoseSup.getPose2d();
+        Pose2d targetPose = new Pose2d();
+        double distance = Double.MAX_VALUE;
 
-        command.addRequirements(swerveSubsystem);
-
-        return command;
+        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};
+
+        for (Pose2d possibleTarget : possibleTargets) {
+            double possibleDistance = currentPose.getTranslation().getDistance(possibleTarget.getTranslation());
+            if (possibleDistance < distance) {
+                distance = possibleDistance;
+                targetPose = possibleTarget;
+            }
+        }
+
+        return getAlignCommand(targetPose, swerveSubsystem);
     }
 }

src/main/java/frc/robot/controllers/BaseDriveController.java

@@ -88,4 +88,7 @@ public abstract class BaseDriveController {
      * Gets the button to aim the shooter at the speaker.
      */
     public abstract JoystickButton getShooterAimButton();
+
+    /** Returns the button to auto-align to the nearest stage face. */
+    public abstract JoystickButton getStageAlignButton();
 }

src/main/java/frc/robot/controllers/DualJoystickDriveController.java

@@ -15,6 +15,7 @@ public class DualJoystickDriveController extends BaseDriveController {
     private final JoystickButton leftMiddleButton = new JoystickButton(leftJoystick, 2);
     private final JoystickButton leftTopLeftButton = new JoystickButton(leftJoystick, 3);
     private final JoystickButton leftTopRightButton = new JoystickButton(leftJoystick, 4);
+    private final JoystickButton leftBottomLeftButton = new JoystickButton(leftJoystick, 5);
 
     private final Joystick rightJoystick = new Joystick(1);
     private final JoystickButton rightTrigger = new JoystickButton(rightJoystick, 1);
@@ -100,5 +101,10 @@ public Boolean getSwerveAimMode() {
     @Override
     public JoystickButton getShooterAimButton() {
         return leftTopRightButton;
-    }   
+    }
+
+    @Override
+    public JoystickButton getStageAlignButton() {
+        return leftBottomLeftButton;
+    }
 }   

src/main/java/frc/robot/controllers/XboxDriveController.java

@@ -21,6 +21,9 @@ public class XboxDriveController extends BaseDriveController {
     private final JoystickButton driveLStickButton = new JoystickButton(
         driveController, XboxController.Button.kLeftStick.value
     );
+    private final JoystickButton driveRStickButton = new JoystickButton(
+        driveController, XboxController.Button.kRightStick.value
+    );
 
     @Override
     public double getForwardPower() {
@@ -77,7 +80,13 @@ public Boolean getSwerveAimMode() {
         return driveLStickButton.getAsBoolean();
     }
 
-    public JoystickButton getShooterAimButton(){
+    @Override
+    public JoystickButton getShooterAimButton() {
         return driveLStickButton;
     }
+
+    @Override
+    public JoystickButton getStageAlignButton() {
+        return  driveRStickButton;
+    }
 }