diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a243fd05..ca5c8309 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -16,6 +16,7 @@
import frc.robot.commands.drivetrain.ResetGyro;
import frc.robot.commands.drivetrain.WheelAlign;
import frc.robot.commands.ramp.ResetRamp;
+import frc.robot.commands.sequences.StartIntakeAndFeeder;
import frc.robot.constants.Constants;
import frc.robot.utils.TimeoutCounter;
import frc.robot.utils.diag.Diagnostics;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cd040a67..5f71b6c6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -314,6 +314,10 @@ public Command getAutoCommand() {
return autoChooser.getAutoCommand();
}
+ public Feeder getFeeder() {
+ return feeder;
+ }
+
/**
* Returns a boolean based on the current alliance color assigned by the FMS.
*
@@ -328,7 +332,7 @@ public AutoChooser getAutoChooser() {
return autoChooser;
}
- public Feeder getFeeder() {
- return feeder;
+ public IntakeSubsystem getIntake() {
+ return intake;
}
}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java b/src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java
index 6fd0fe24..18b5b972 100644
--- a/src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java
+++ b/src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java
@@ -1,43 +1,47 @@
package frc.robot.commands.sequences;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.commands.deployer.LowerDeployer;
import frc.robot.commands.deployer.RaiseDeployer;
+import frc.robot.commands.feeder.FeederBackDrive;
import frc.robot.commands.feeder.StartFeeder;
import frc.robot.commands.intake.StartIntake;
-import frc.robot.commands.feeder.FeederBackDrive;
import frc.robot.commands.ramp.RampMoveAndWait;
import frc.robot.constants.GameConstants;
import frc.robot.subsystems.Deployer;
import frc.robot.subsystems.Feeder;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.Ramp;
+import frc.robot.utils.logging.CommandUtil;
+import frc.robot.utils.logging.SequentialLoggingCommand;
/**
* Sequence to start intaking, this takes care of the lowering/raising the deployer,
* as well as starting/stopping the intake and feeder.
*/
-public class StartIntakeAndFeeder extends SequentialCommandGroup{
+public class StartIntakeAndFeeder extends SequentialLoggingCommand {
+ @Override
+ public void execute() {
+ super.execute();
+ }
+
public StartIntakeAndFeeder(Feeder feeder, IntakeSubsystem intake, Deployer deployer, Ramp ramp) {
- addCommands(
- new ParallelCommandGroup(
- new LowerDeployer(deployer),
- new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
- ),
- new ParallelRaceGroup(
- new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
- new StartFeeder(feeder)
- ),
- new ParallelCommandGroup(
- new RaiseDeployer(deployer),
- new SequentialCommandGroup(
- new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
- new FeederBackDrive(feeder)
+ super("StartIntakeAndFeeder",
+ CommandUtil.parallel("First",
+ new LowerDeployer(deployer),
+ new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
+ ),
+ CommandUtil.race("second",
+ new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
+ new StartFeeder(feeder)
+ ),
+ CommandUtil.parallel("third",
+ new RaiseDeployer(deployer),
+ CommandUtil.sequence("Fourth",
+ new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
+ new FeederBackDrive(feeder)
+ )
)
- )
);
}
}
diff --git a/src/main/java/frc/robot/utils/logging/CommandUtil.java b/src/main/java/frc/robot/utils/logging/CommandUtil.java
index 5ac78ab7..f375d170 100644
--- a/src/main/java/frc/robot/utils/logging/CommandUtil.java
+++ b/src/main/java/frc/robot/utils/logging/CommandUtil.java
@@ -32,6 +32,18 @@
* Command pickUp = CommandUtil.sequence("ArmSequence", new OpenGripper(), new RaiseArm(), new CloseGripper());
* Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
*
+ *
+ * and also for nesting command groups that are custom-created:
+ * (Note the "extends SequentialLoggingCommandGroup" - DO NOT extend regular SequentialCommandGroup or this will not work)
+ *
+ * class PickupGroup extends SequentialLoggingCommandGroup {
+ * public PickupGroup() {
+ * super("Pickup", new OpenGripper(), new RaiseArm(), new CloseGripper());
+ * }
+ * }
+ * Command pickUp = new PickupGroup();
+ * Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
+ *
*/
public class CommandUtil {
public static final String COMMAND_PREFIX = "Commands";
@@ -52,6 +64,8 @@ public static Command logged(Command command) {
/**
* Make a logging command from the given command, with a custom name hierarchy.
* The logging command will be placed within the given name prefix naming hierarchy instead of at the root.
+ * @param namePrefix the nesting hierarchy prefix for the command. If null the command is going to log at the
+ * root level. If not null it will be nested in this namespace.
* @param command the command to wrap
* @return a {@link LoggingCommand} that wraps the given command
*/
@@ -76,30 +90,27 @@ public static Command runOnce(String name, Runnable action, Subsystem... require
/**
* Return a loggable {@link SequentialCommandGroup} that executes the given commands sequentially. The given commands
* can be any command or command group - they will be converted to loggable commands implicitly.
- * @param sequenceName the name of the resaulting command group
+ * @param sequenceName the name of the resulting command group
* @param commands the commands to put in the group
* @return the created loggable command group
*/
public static Command sequence(String sequenceName, Command... commands) {
- LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
- return new SequentialLoggingCommand(sequenceName, loggingCommands);
+ return new SequentialLoggingCommand(sequenceName, commands);
}
/**
* Return a loggable {@link ParallelCommandGroup} that executes the given commands in parallel. The given commands
* can be any command or command group - they will be converted to loggable commands implicitly.
- * @param sequenceName the name of the resaulting command group
+ * @param sequenceName the name of the resulting command group
* @param commands the commands to put in the group
* @return the created loggable command group
*/
public static Command parallel(String sequenceName, Command... commands) {
- LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
- return new ParallelLoggingCommand(sequenceName, loggingCommands);
+ return new ParallelLoggingCommand(sequenceName, commands);
}
public static Command race(String sequenceName, Command... commands) {
- LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
- return new RaceLoggingCommand(sequenceName, loggingCommands);
+ return new RaceLoggingCommand(sequenceName, commands);
}
public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) {
@@ -113,9 +124,7 @@ public static LoggingCommand[] wrapForLogging(String prefix, Command... commands
private static LoggingCommand wrapForLogging(String prefix, Command command) {
if (command instanceof LoggingCommand loggingCommand) {
- // change the prefix to include the current new parent
- String childPrefix = prefix + CommandUtil.NAME_SEPARATOR + loggingCommand.getNamePrefix();
- loggingCommand.setNamePrefix(childPrefix);
+ loggingCommand.appendNamePrefix(prefix);
return loggingCommand;
} else {
// New command located in the given sequence root
diff --git a/src/main/java/frc/robot/utils/logging/GroupLoggingCommand.java b/src/main/java/frc/robot/utils/logging/GroupLoggingCommand.java
new file mode 100644
index 00000000..7b8cb407
--- /dev/null
+++ b/src/main/java/frc/robot/utils/logging/GroupLoggingCommand.java
@@ -0,0 +1,69 @@
+package frc.robot.utils.logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Base class for logged grouped commands (parallel, race, sequential...)
+ */
+public abstract class GroupLoggingCommand extends LoggingCommand {
+ // A copy of the children (since we can't access them from the regular groups)
+ private final List childLoggingCommands;
+
+ /**
+ * Constructor for logged group command.
+ *
+ * @param namePrefix the prefix for the name (comes in front of hte group name)
+ * @param underlying the group command that is wrapped by this command
+ */
+ public GroupLoggingCommand(String namePrefix, String commandName, Command underlying) {
+ super(namePrefix, commandName, underlying);
+ childLoggingCommands = new ArrayList<>();
+ }
+
+ /**
+ * A special constructor to allow for the creation of the command when we can't create the underlying up front.
+ * It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
+ */
+ protected GroupLoggingCommand(String namePrefix, String commandName) {
+ super(namePrefix, commandName);
+ childLoggingCommands = new ArrayList<>();
+ }
+
+ public final void addLoggingCommands(List commands) {
+ childLoggingCommands.addAll(commands);
+ }
+
+ @Override
+ public void setName(String name) {
+ // Do not change the logging name for this command (it is fixed)
+ getUnderlying().setName(name);
+ }
+
+ @Override
+ public void appendNamePrefix(String prefix) {
+ // Change the name for this command
+ super.appendNamePrefix(prefix);
+ // Change the name for the children
+ appendChildrenPrefix(prefix);
+ }
+
+ @Override
+ public String toString() {
+ return getFullyQualifiedName();
+ }
+
+ // For testing
+ public List getChildLoggingCommands() {
+ return childLoggingCommands;
+ }
+
+ private void appendChildrenPrefix(String prefix) {
+ // Recursively change the prefix for all child commands
+ for (LoggingCommand loggingCommand : childLoggingCommands) {
+ loggingCommand.appendNamePrefix(prefix);
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/utils/logging/LoggingCommand.java b/src/main/java/frc/robot/utils/logging/LoggingCommand.java
index 442790d9..4ef5d2c3 100644
--- a/src/main/java/frc/robot/utils/logging/LoggingCommand.java
+++ b/src/main/java/frc/robot/utils/logging/LoggingCommand.java
@@ -9,10 +9,14 @@
import java.util.Set;
public abstract class LoggingCommand extends Command {
+ // Prefix for the command name (appended ahead of the command name) - this can be appended to when the command gets
+ // embedded at another command through appendNamePrefix
private String namePrefix;
+ // Private name for the command (last in the hierarchy for this command) - this can be changed through setName
private String commandName;
- private final Command underlying;
-
+ // The delegate command where we refer all calls
+ private Command underlying;
+ // The name as it would be used in the logs
private String fullyQualifiedName;
// Lazy initialized log entry to make sure we don't create it until it is needed (and command is scheduled)
// otherwise we get an empty line in the log visualization tool
@@ -25,6 +29,23 @@ public LoggingCommand(String namePrefix, String commandName, Command underlying)
resetLoggingName(namePrefix, commandName);
}
+ /**
+ * A special constructor to allow for the creation of the command when we can't create the underlying up front.
+ * It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
+ */
+ protected LoggingCommand(String namePrefix, String commandName) {
+ this.namePrefix = namePrefix;
+ this.commandName = commandName;
+ resetLoggingName(namePrefix, commandName);
+ }
+
+ /**
+ * Second phase of initialization - set the underlying command
+ */
+ protected void setUnderlying(Command underlying) {
+ this.underlying = underlying;
+ }
+
@Override
public void initialize() {
if (Constants.ENABLE_LOGGING) getLoggingEntry().append(true);
@@ -83,8 +104,12 @@ public String getNamePrefix() {
return namePrefix;
}
- public void setNamePrefix(String prefix) {
- this.namePrefix = prefix;
+ /**
+ * Add a new prefix in front of the current one
+ * @param prefix the new prefix
+ */
+ public void appendNamePrefix(String prefix) {
+ this.namePrefix = prefix + CommandUtil.NAME_SEPARATOR + namePrefix;
resetLoggingName(namePrefix, commandName);
}
@@ -112,3 +137,5 @@ private BooleanLogEntry getLoggingEntry() {
return logEntry;
}
}
+
+
diff --git a/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java b/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java
new file mode 100644
index 00000000..105a4db5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java
@@ -0,0 +1,41 @@
+package frc.robot.utils.logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+
+import java.util.Arrays;
+
+public class ParallelDeadlineLoggingCommand extends GroupLoggingCommand {
+ private LoggingCommand deadlineLoggingCommand;
+
+ /**
+ * Constructor for parallel deadline command group.
+ *
+ * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+ * @param deadline the deadline command - the one that when ends, stops other commands
+ * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK)
+ */
+ public ParallelDeadlineLoggingCommand(String namePrefix, Command deadline, Command... commands) {
+ // Since we can't initialize the deadlineGroup with empty commands, use the special constructor to pass in the
+ // underlying afterward
+ super(namePrefix, "(Deadline)");
+
+ deadlineLoggingCommand = CommandUtil.wrapForLogging(namePrefix, deadline)[0];
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+ setUnderlying(new ParallelDeadlineGroup(deadlineLoggingCommand, wrapped));
+ addLoggingCommands(Arrays.asList(wrapped));
+ }
+
+ public final void addCommands(Command... commands) {
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+ ((ParallelDeadlineGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
+ }
+
+ @Override
+ public void appendNamePrefix(String prefix) {
+ super.appendNamePrefix(prefix);
+ // Change the name for the deadline
+ deadlineLoggingCommand.appendNamePrefix(prefix);
+ }
+}
diff --git a/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java b/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java
index 7bb84064..7c5c7ebf 100644
--- a/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java
+++ b/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java
@@ -1,38 +1,29 @@
package frc.robot.utils.logging;
+import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-public class ParallelLoggingCommand extends LoggingCommand {
- private static final String THIS_NAME = "-this";
-
- private LoggingCommand[] loggingCommands;
-
- public ParallelLoggingCommand(String namePrefix, LoggingCommand... commands) {
- super(namePrefix, THIS_NAME, new ParallelCommandGroup(commands));
- this.loggingCommands = commands;
- }
-
- @Override
- public void setName(String name) {
- // Do not change the logging name for this command (it is fixed)
- getUnderlying().setName(name);
- }
-
- @Override
- public void setNamePrefix(String prefix) {
- super.setNamePrefix(prefix);
- setChildrenPrefix(prefix);
- }
-
- @Override
- public String toString() {
- return getFullyQualifiedName();
+import java.util.Arrays;
+
+public class ParallelLoggingCommand extends GroupLoggingCommand {
+ /**
+ * Constructor for parallel command group.
+ *
+ * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+ * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK)
+ */
+ public ParallelLoggingCommand(String namePrefix, Command... commands) {
+ // Call super with an empty group, populate children afterward
+ super(namePrefix, "(Parallel)", new ParallelCommandGroup());
+
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+ ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
- private void setChildrenPrefix(String prefix) {
- // Recursively change the prefix for all child commands
- for (LoggingCommand loggingCommand : loggingCommands) {
- loggingCommand.setNamePrefix(prefix);
- }
+ public final void addCommands(Command... commands) {
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+ ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
}
diff --git a/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java b/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java
index 60bd9fba..21ff8c89 100644
--- a/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java
+++ b/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java
@@ -1,38 +1,29 @@
package frc.robot.utils.logging;
+import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-public class RaceLoggingCommand extends LoggingCommand {
- private static final String THIS_NAME = "-this";
-
- private LoggingCommand[] loggingCommands;
-
- public RaceLoggingCommand(String namePrefix, LoggingCommand... commands) {
- super(namePrefix, THIS_NAME, new ParallelRaceGroup(commands));
- this.loggingCommands = commands;
- }
-
- @Override
- public void setName(String name) {
- // Do not change the logging name for this command (it is fixed)
- getUnderlying().setName(name);
- }
-
- @Override
- public void setNamePrefix(String prefix) {
- super.setNamePrefix(prefix);
- setChildrenPrefix(prefix);
- }
-
- @Override
- public String toString() {
- return getFullyQualifiedName();
+import java.util.Arrays;
+
+public class RaceLoggingCommand extends GroupLoggingCommand {
+ /**
+ * Constructor for race command group.
+ *
+ * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+ * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK)
+ */
+ public RaceLoggingCommand(String namePrefix, Command... commands) {
+ // Call super with an empty group, populate children afterward
+ super(namePrefix, "(Race)", new ParallelRaceGroup());
+
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+ ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
- private void setChildrenPrefix(String prefix) {
- // Recursively change the prefix for all child commands
- for (LoggingCommand loggingCommand : loggingCommands) {
- loggingCommand.setNamePrefix(prefix);
- }
+ public final void addCommands(Command... commands) {
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+ ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
}
diff --git a/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java b/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java
index 6ca3aa97..983d6c79 100644
--- a/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java
+++ b/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java
@@ -1,38 +1,29 @@
package frc.robot.utils.logging;
+import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-public class SequentialLoggingCommand extends LoggingCommand {
- private static final String THIS_NAME = "-this";
-
- private final LoggingCommand[] loggingCommands;
-
- public SequentialLoggingCommand(String namePrefix, LoggingCommand... commands) {
- super(namePrefix, THIS_NAME, new SequentialCommandGroup(commands));
- this.loggingCommands = commands;
- }
-
- @Override
- public void setName(String name) {
- // Do not change the logging name for this command (it is fixed)
- getUnderlying().setName(name);
- }
-
- @Override
- public void setNamePrefix(String prefix) {
- super.setNamePrefix(prefix);
- setChildrenPrefix(prefix);
- }
-
- @Override
- public String toString() {
- return getFullyQualifiedName();
+import java.util.Arrays;
+
+public class SequentialLoggingCommand extends GroupLoggingCommand {
+ /**
+ * Constructor for sequential command group.
+ *
+ * @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
+ * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK)
+ */
+ public SequentialLoggingCommand(String namePrefix, Command... commands) {
+ // Call super with an empty group, populate children afterward
+ super(namePrefix, "(Sequence)", new SequentialCommandGroup());
+
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
+ ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
- private void setChildrenPrefix(String prefix) {
- // Recursively change the prefix for all child commands
- for (LoggingCommand loggingCommand : loggingCommands) {
- loggingCommand.setNamePrefix(prefix);
- }
+ public final void addCommands(Command... commands) {
+ LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
+ ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped);
+ addLoggingCommands(Arrays.asList(wrapped));
}
}