AutoAlgaeHeights Working

Author: RobototesProgrammers

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

@@ -61,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;
 
@@ -950,7 +950,7 @@ private void configureSoloControllerBindings() {
                                   .alongWith(scoringModeSelectRumble())
                                   .withName("Algae Scoring Mode"),
                               AutoAlgaeHeights.autoAlgaeIntakeCommand(
-                                      s.drivebaseSubsystem, superStructure)
+                                      s.drivebaseSubsystem, superStructure, this)
                                   .until(() -> sensors.armSensor.booleanInClaw()));
                     }
                     default -> scoreCommand = Commands.none();
@@ -997,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

@@ -6,8 +6,10 @@
 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;
 
 /**
@@ -18,6 +20,7 @@ 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);
@@ -40,14 +43,15 @@ public class AutoAlgaeHeights extends Command {
           aprilTagFieldLayout.getTagPose(9).get().toPose2d(),
           aprilTagFieldLayout.getTagPose(11).get().toPose2d());
 
-  public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s) {
+  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) {
-    return new AutoAlgaeHeights(drivebase, s);
+      CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) {
+    return new AutoAlgaeHeights(drivebase, s, c);
   }
 
   private static boolean isBlue() {
@@ -95,6 +99,6 @@ public void execute() {
 
   @Override
   public boolean isFinished() {
-    return false;
+    return c.intakeMode != ScoringMode.ALGAE;
   }
 }