Algae automatic height switching

src/main/java/frc/robot/Controls.java

@@ -30,6 +30,7 @@
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.GroundArm;
 import frc.robot.subsystems.SuperStructure;
+import frc.robot.subsystems.auto.AutoAlgaeHeights;
 import frc.robot.subsystems.auto.AutoAlign;
 import frc.robot.subsystems.auto.BargeAlign;
 import frc.robot.util.AlgaeIntakeHeight;
@@ -60,7 +61,7 @@ public class Controls {
 
   private BranchHeight branchHeight = BranchHeight.CORAL_LEVEL_FOUR;
   private ScoringMode scoringMode = ScoringMode.CORAL;
-  private ScoringMode intakeMode = ScoringMode.CORAL;
+  public ScoringMode intakeMode = ScoringMode.CORAL;
   private SoloScoringMode soloScoringMode = SoloScoringMode.NO_GAME_PIECE;
   private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR;
 
@@ -919,31 +920,43 @@ private void configureSoloControllerBindings() {
     soloController
         .leftTrigger()
         .onTrue(
-            Commands.deferredProxy(
-                () ->
-                    switch (soloScoringMode) {
-                      case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand();
-                      case ALGAE_IN_CLAW -> Commands.sequence(
-                              BargeAlign.bargeScore(
+            Commands.runOnce(
+                () -> {
+                  Command scoreCommand;
+                  switch (soloScoringMode) {
+                    case CORAL_IN_CLAW -> {
+                      scoreCommand = getSoloCoralBranchHeightCommand();
+                    }
+                    case ALGAE_IN_CLAW -> {
+                      Command bargeScoreCommand =
+                          BargeAlign.bargeScore(
                                   s.drivebaseSubsystem,
                                   superStructure,
                                   () -> getSoloDriveX(),
                                   () -> getSoloDriveY(),
                                   () -> getSoloDriveRotate(),
-                                  soloController.rightBumper()),
+                                  soloController.rightBumper())
+                              .withName("Algae score then intake");
+                      scoreCommand =
+                          Commands.sequence(
+                              bargeScoreCommand,
                               Commands.runOnce(
-                                  () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE))
-                          .withName("Algae score then intake");
-                      case NO_GAME_PIECE -> Commands.parallel(
-                          Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE)
-                              .alongWith(scoringModeSelectRumble())
-                              .withName("Algae Scoring Mode"),
-                          Commands.runOnce(
-                                  () ->
-                                      CommandScheduler.getInstance()
-                                          .schedule(getAlgaeIntakeCommand()))
-                              .withName("Run Algae Intake"));
-                    }));
+                                  () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE));
+                    }
+                    case NO_GAME_PIECE -> {
+                      scoreCommand =
+                          Commands.parallel(
+                              Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE)
+                                  .alongWith(scoringModeSelectRumble())
+                                  .withName("Algae Scoring Mode"),
+                              AutoAlgaeHeights.autoAlgaeIntakeCommand(
+                                      s.drivebaseSubsystem, superStructure, this)
+                                  .until(() -> sensors.armSensor.booleanInClaw()));
+                    }
+                    default -> scoreCommand = Commands.none();
+                  }
+                  CommandScheduler.getInstance().schedule(scoreCommand);
+                }));
     soloController
         .leftTrigger()
         .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
@@ -984,8 +997,9 @@ private void configureSoloControllerBindings() {
     soloController
         .leftBumper()
         .onTrue(
-            superStructure
-                .quickGroundIntake(soloController.povUp())
+            Commands.parallel(
+                    Commands.runOnce(() -> intakeMode = ScoringMode.CORAL),
+                    superStructure.quickGroundIntake(soloController.povUp()))
                 .withName("Quick Gound intake"));
     // Scoring levels coral and algae intake heights
     soloController

src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java

@@ -0,0 +1,104 @@
+package frc.robot.subsystems.auto;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Controls;
+import frc.robot.subsystems.SuperStructure;
+import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
+import frc.robot.util.ScoringMode;
+import java.util.List;
+
+/**
+ * Command that automatically moves the elevator to the proper height for the nearest algae based on
+ * alliance and current robot pose.
+ */
+public class AutoAlgaeHeights extends Command {
+
+  private final CommandSwerveDrivetrain drivebase;
+  private final SuperStructure s;
+  private final Controls c;
+
+  private static final AprilTagFieldLayout aprilTagFieldLayout =
+      AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
+  private static final List<Pose2d> blueAlgaePoses =
+      List.of(
+          aprilTagFieldLayout.getTagPose(17).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(19).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(21).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(18).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(20).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(22).get().toPose2d());
+
+  private static final List<Pose2d> redAlgaePoses =
+      List.of(
+          aprilTagFieldLayout.getTagPose(6).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(8).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(10).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(7).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(9).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(11).get().toPose2d());
+
+  public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) {
+    this.drivebase = drivebase;
+    this.s = s;
+    this.c = c;
+  }
+
+  public static Command autoAlgaeIntakeCommand(
+      CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) {
+    return new AutoAlgaeHeights(drivebase, s, c);
+  }
+
+  private static boolean isBlue() {
+    return DriverStation.getAlliance()
+        .map(alliance -> alliance.equals(Alliance.Blue))
+        .orElse(false);
+  }
+
+  private static Pose2d getNearestAlgae(Pose2d robotPose) {
+    List<Pose2d> poses = isBlue() ? blueAlgaePoses : redAlgaePoses;
+    return robotPose.nearest(poses);
+  }
+
+  private String aprilTagToAlgaeHeight(Pose2d algaePose) {
+    // Map poses to two heights only
+    List<Pose2d> poses = isBlue() ? blueAlgaePoses : redAlgaePoses;
+    int index = poses.indexOf(algaePose);
+
+    if (index >= 3) {
+      return "top"; // upper reef
+    } else {
+      return "low"; // lower reef
+    }
+  }
+
+  private String activeCommand = "";
+
+  @Override
+  public void execute() {
+    Pose2d robotPose = drivebase.getState().Pose;
+    Pose2d nearestAlgae = getNearestAlgae(robotPose);
+
+    String targetHeight = aprilTagToAlgaeHeight(nearestAlgae);
+
+    if (!(activeCommand.equals(targetHeight))) {
+      if (targetHeight.equals("top")) {
+        s.algaeLevelThreeFourIntake().schedule();
+        activeCommand = "top";
+      } else {
+        s.algaeLevelTwoThreeIntake().schedule();
+        activeCommand = "low";
+      }
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return c.intakeMode != ScoringMode.ALGAE;
+  }
+}