Manual Align Redundancy

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

@@ -4,7 +4,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -25,6 +24,7 @@
 import frc.robot.Constants.Mode;
 import frc.robot.commands.AdaptiveAutoAlignCommands;
 import frc.robot.commands.DriveCommands;
+import frc.robot.commands.ManualAlignCommands;
 import frc.robot.commands.controllers.JoystickInputController;
 import frc.robot.commands.controllers.SpeedLevelController;
 import frc.robot.subsystems.dashboard.DriverDashboard;
@@ -63,10 +63,11 @@
 import frc.robot.subsystems.vision.CameraIOPhotonVision;
 import frc.robot.subsystems.vision.CameraIOSim;
 import frc.robot.subsystems.vision.VisionConstants;
+import frc.robot.utility.JoystickUtil;
 import frc.robot.utility.OverrideSwitch;
 import frc.robot.utility.commands.CustomCommands;
 import java.util.Arrays;
-import java.util.function.DoubleSupplier;
+import java.util.function.BiConsumer;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -397,6 +398,18 @@ private void configureDriverControllerBindings(
                 .ignoringDisable(true)
                 .withName("Reset Gyro Heading"));
 
+    final BiConsumer<Trigger, Command> configureAlignmentAuto =
+        (trigger, command) -> {
+          trigger.onTrue(command.until(() -> input.getOmegaRadiansPerSecond() != 0));
+        };
+
+    configureAlignmentAuto.accept(
+        xbox.povRight(), ManualAlignCommands.alignToSourceRight(drive, input));
+    configureAlignmentAuto.accept(
+        xbox.povLeft(), ManualAlignCommands.alignToSourceLeft(drive, input));
+    configureAlignmentAuto.accept(xbox.povDown(), ManualAlignCommands.alignToCageAdv(drive, input));
+    configureAlignmentAuto.accept(xbox.povUp(), ManualAlignCommands.alignToReef(drive, input));
+
     if (includeAutoAlign) {
       // Align to reef
 
@@ -477,34 +490,35 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
     xbox.rightTrigger().and(xbox.a().negate()).whileTrue(superstructure.outtake());
     xbox.rightTrigger().and(xbox.a()).whileTrue(superstructure.outtakeL1());
 
-    configureOperatorControllerBindingLevel(xbox.y(), Superstructure.State.L4);
-    configureOperatorControllerBindingLevel(xbox.x(), Superstructure.State.L3);
-    configureOperatorControllerBindingLevel(xbox.a(), Superstructure.State.L2);
+    final BiConsumer<Trigger, Superstructure.State> configureOperatorControllerBindingLevel =
+        (trigger, level) -> {
+          trigger.whileTrue(superstructure.run(level));
+        };
 
-    configureOperatorControllerBindingLevel(xbox.b(), Superstructure.State.L1);
+    configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
+    configureOperatorControllerBindingLevel.accept(xbox.x(), Superstructure.State.L3);
+    configureOperatorControllerBindingLevel.accept(xbox.a(), Superstructure.State.L2);
+    configureOperatorControllerBindingLevel.accept(xbox.b(), Superstructure.State.L1);
 
     xbox.povDown().onTrue(superstructure.stowLow());
 
+    // Intake
+
     coralIntake.setDefaultCommand(superstructure.passiveIntake());
+    new Trigger(() -> !JoystickUtil.isDeadband(xbox.getLeftX()))
+        .and(DriverStation::isTeleopEnabled)
+        .whileTrue(
+            Commands.run(
+                () -> coralIntake.setMotors(JoystickUtil.applyDeadband(xbox.getRightX()))));
 
     // Hang
-    hang.setDefaultCommand(hang.run(() -> hang.set(MathUtil.applyDeadband(xbox.getLeftX(), 0.2))));
-
-    DoubleSupplier hangSpeed = () -> MathUtil.applyDeadband(xbox.getRightX(), 0.2);
-    new Trigger(() -> hangSpeed.getAsDouble() != 0)
-        .and(DriverStation::isTeleopEnabled)
-        .whileTrue(Commands.run(() -> hang.set(hangSpeed.getAsDouble())));
+    hang.setDefaultCommand(hang.run(() -> hang.set(JoystickUtil.applyDeadband(xbox.getLeftX()))));
 
     xbox.rightBumper().and(xbox.leftBumper().negate()).onTrue(hang.deploy());
     xbox.leftBumper().and(xbox.rightBumper().negate()).onTrue(hang.retract());
     xbox.rightBumper().and(xbox.leftBumper()).onTrue(hang.stow());
   }
 
-  private void configureOperatorControllerBindingLevel(
-      Trigger trigger, Superstructure.State state) {
-    trigger.whileTrue(superstructure.run(state));
-  }
-
   private Command rumbleController(CommandXboxController controller, double rumbleIntensity) {
     return Commands.startEnd(
             () -> controller.setRumble(RumbleType.kBothRumble, rumbleIntensity),

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

@@ -0,0 +1,95 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+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(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);
+  }
+
+  public static Command alignToSourceLeft(Drive drivetrain, JoystickInputController input) {
+    return DriveCommands.joystickHeadingDrive(
+        drivetrain,
+        input::getTranslationMetersPerSecond,
+        () -> Optional.of(AllianceFlipUtil.apply(CoralStation.leftCenterFace.getRotation())),
+        () -> SpeedLevel.NO_LEVEL,
+        () -> true);
+  }
+
+  public static Command alignToSourceRight(Drive drivetrain, JoystickInputController input) {
+    return DriveCommands.joystickHeadingDrive(
+        drivetrain,
+        input::getTranslationMetersPerSecond,
+        () -> Optional.of(AllianceFlipUtil.apply(CoralStation.rightCenterFace.getRotation())),
+        () -> SpeedLevel.NO_LEVEL,
+        () -> true);
+  }
+
+  public static Command alignToReef(Drive drivetrain, JoystickInputController input) {
+    return DriveCommands.joystickHeadingDrive(
+        drivetrain,
+        input::getTranslationMetersPerSecond,
+        () -> {
+          Translation2d robotPose = drivetrain.getRobotPose().getTranslation();
+          Translation2d reefPose = AllianceFlipUtil.apply(Reef.center);
+
+          double angleRad = robotPose.minus(reefPose).getAngle().getRadians();
+
+          for (Pose2d face : Reef.alignmentFaces) {
+            double faceRad = face.getRotation().getRadians();
+
+            if (MathUtil.isNear(faceRad, angleRad, Units.degreesToRadians(25))) {
+              angleRad = faceRad;
+            }
+          }
+
+          return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromRadians(angleRad)));
+        },
+        () -> SpeedLevel.NO_LEVEL,
+        () -> true);
+  }
+
+  private static Rotation2d allianceRotation() {
+    return AllianceFlipUtil.shouldFlip() ? Rotation2d.kZero : Rotation2d.k180deg;
+  }
+}

src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOHardware.java

@@ -18,7 +18,6 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.superstructure.wrist.WristConstants.WristConfig;
 import frc.robot.utility.SparkUtil;
 

src/main/java/frc/robot/utility/JoystickUtil.java

@@ -16,4 +16,8 @@ public static double applySimpleDeadband(double value, double deadband) {
   public static double applyDeadband(double value) {
     return MathUtil.applyDeadband(value, JOYSTICK_DEADBAND);
   }
+
+  public static boolean isDeadband(double value) {
+    return Math.abs(value) < JOYSTICK_DEADBAND;
+  }
 }