Add Loop Command

src/main/java/frc4388/robot/RobotContainer.java

@@ -46,6 +46,7 @@ import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.WhileTrueCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
 import frc4388.robot.commands.waitEndefectorRefrence;
 import frc4388.robot.commands.waitFeedCoral;
@@ -316,6 +317,12 @@ public class RobotContainer {
     
     private Boolean operatorManualMode = false;
     
+    public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral());
+    public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral());
+    public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral());
+    public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral());
+    public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral());
+    public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral());
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -330,12 +337,12 @@ public class RobotContainer {
                 new Translation2d(), 400, true
         ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
         
-        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
+        NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left);
-        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
+        NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right);
-        NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
+        NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left);
-        NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
+        NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right);
-        NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
+        NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left);
-        NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
+        NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right);
 
         NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
             m_robotElevator.transitionState(CoordinationState.PrimedFour);
@@ -639,6 +646,8 @@ public class RobotContainer {
         // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
         String[] autos = dir.list();
 
+        if(autos == null) return;
+
         for (String auto : autos) {
             if (auto.endsWith(".auto"))
                 autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));

src/main/java/frc4388/robot/commands/WhileTrueCommand.java

@@ -0,0 +1,103 @@
+package frc4388.robot.commands;
+
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command composition that runs one of two commands, depending on the value of the given
+ * condition when this command is initialized.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WhileTrueCommand extends Command {
+  private final Command m_whileTrue;
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WhileTrueCommand.
+   *
+   * @param whileTrue the command to run while the condition is true
+   * @param condition the condition to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
+    m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand");
+    m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
+
+    CommandScheduler.getInstance().registerComposedCommands(whileTrue);
+
+    addRequirements(whileTrue.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if(m_condition.getAsBoolean())
+     m_whileTrue.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_whileTrue.execute();
+
+    if(!m_whileTrue.isFinished())
+      return;
+
+    if(m_condition.getAsBoolean()){
+      m_whileTrue.end(false);
+      m_whileTrue.initialize();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_whileTrue.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_whileTrue.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+      return InterruptionBehavior.kCancelSelf;
+    } else {
+      return InterruptionBehavior.kCancelIncoming;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
+    builder.addStringProperty(
+        "selected",
+        () -> {
+          if (m_whileTrue == null) {
+            return "null";
+          } else {
+            return m_whileTrue.getName();
+          }
+        },
+        null);
+  }
+}