From aeedebd0955a49903893fffffa8bce6358627bc8 Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Wed, 5 Mar 2025 13:07:28 -0700
Subject: [PATCH] Add Loop Command
---
.../java/frc4388/robot/RobotContainer.java | 21 +++-
.../robot/commands/WhileTrueCommand.java | 103 ++++++++++++++++++
2 files changed, 118 insertions(+), 6 deletions(-)
create mode 100644 src/main/java/frc4388/robot/commands/WhileTrueCommand.java
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f214584..c938162 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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-right-l4", AprilLidarAlignL4Right);
- NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
- NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
- NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
- NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
+ NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left);
+ NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right);
+ NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left);
+ NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right);
+ NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left);
+ 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", ""));
diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java
new file mode 100644
index 0000000..31b189e
--- /dev/null
+++ b/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.
+ *
+ * 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.
+ *
+ *
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);
+ }
+}