From edb7e7144de5010bb589ba170ae0128c145454b6 Mon Sep 17 00:00:00 2001 From: ohad Date: Sun, 3 Mar 2024 17:03:06 -0500 Subject: [PATCH 1/5] Enable constructor for logging command groups to take regular commands and wrap them --- .../frc/robot/utils/logging/CommandUtil.java | 15 +++++---- .../utils/logging/ParallelLoggingCommand.java | 26 +++++++++++++--- .../utils/logging/RaceLoggingCommand.java | 26 +++++++++++++--- .../logging/SequentialLoggingCommand.java | 31 ++++++++++++++++--- 4 files changed, 78 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/utils/logging/CommandUtil.java b/src/main/java/frc/robot/utils/logging/CommandUtil.java index 5ac78ab7..0565abb9 100644 --- a/src/main/java/frc/robot/utils/logging/CommandUtil.java +++ b/src/main/java/frc/robot/utils/logging/CommandUtil.java @@ -52,6 +52,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 +78,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) { diff --git a/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java b/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java index 7bb84064..af7e4ccf 100644 --- a/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java @@ -1,15 +1,33 @@ package frc.robot.utils.logging; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import java.util.Arrays; +import java.util.List; + public class ParallelLoggingCommand extends LoggingCommand { private static final String THIS_NAME = "-this"; - private LoggingCommand[] loggingCommands; + private List loggingCommands; + + /** + * 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) { + super(namePrefix, THIS_NAME, new ParallelCommandGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); + ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); + this.loggingCommands = Arrays.asList(wrapped); + } - public ParallelLoggingCommand(String namePrefix, LoggingCommand... commands) { - super(namePrefix, THIS_NAME, new ParallelCommandGroup(commands)); - this.loggingCommands = commands; + public final void addCommands(Command... commands) { + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); + ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); + loggingCommands.addAll(Arrays.asList(wrapped)); } @Override diff --git a/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java b/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java index 60bd9fba..0a6c46f9 100644 --- a/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java @@ -1,15 +1,33 @@ package frc.robot.utils.logging; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import java.util.Arrays; +import java.util.List; + public class RaceLoggingCommand extends LoggingCommand { private static final String THIS_NAME = "-this"; - private LoggingCommand[] loggingCommands; + private List loggingCommands; + + /** + * 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) { + super(namePrefix, THIS_NAME, new ParallelRaceGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); + ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped); + this.loggingCommands = Arrays.asList(wrapped); + } - public RaceLoggingCommand(String namePrefix, LoggingCommand... commands) { - super(namePrefix, THIS_NAME, new ParallelRaceGroup(commands)); - this.loggingCommands = commands; + public final void addCommands(Command... commands) { + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); + ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped); + loggingCommands.addAll(Arrays.asList(wrapped)); } @Override diff --git a/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java b/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java index 6ca3aa97..98a4cdb8 100644 --- a/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java @@ -1,15 +1,33 @@ package frc.robot.utils.logging; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import java.util.Arrays; +import java.util.List; + public class SequentialLoggingCommand extends LoggingCommand { private static final String THIS_NAME = "-this"; - private final LoggingCommand[] loggingCommands; + private final List loggingCommands; + + /** + * 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) { + super(namePrefix, THIS_NAME, new SequentialCommandGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); + ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped); + this.loggingCommands = Arrays.asList(wrapped); + } - public SequentialLoggingCommand(String namePrefix, LoggingCommand... commands) { - super(namePrefix, THIS_NAME, new SequentialCommandGroup(commands)); - this.loggingCommands = commands; + public final void addCommands(Command... commands) { + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); + ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped); + loggingCommands.addAll(Arrays.asList(wrapped)); } @Override @@ -29,6 +47,11 @@ public String toString() { return getFullyQualifiedName(); } + // For testing + public List getLoggingCommands() { + return loggingCommands; + } + private void setChildrenPrefix(String prefix) { // Recursively change the prefix for all child commands for (LoggingCommand loggingCommand : loggingCommands) { From 7ba21724e9fbfe131fa46a24218be961d053b2ee Mon Sep 17 00:00:00 2001 From: ohad Date: Sun, 3 Mar 2024 17:10:26 -0500 Subject: [PATCH 2/5] Add comments --- .../java/frc/robot/utils/logging/CommandUtil.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/utils/logging/CommandUtil.java b/src/main/java/frc/robot/utils/logging/CommandUtil.java index 0565abb9..414dff0e 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"; From d39c2f8ead76f195f7ade3ce36ef3a378c1dd390 Mon Sep 17 00:00:00 2001 From: ohad Date: Mon, 4 Mar 2024 23:44:48 -0500 Subject: [PATCH 3/5] Add deadline group --- .../frc/robot/utils/logging/CommandUtil.java | 4 ++ .../robot/utils/logging/LoggingCommand.java | 19 +++++- .../ParallelDeadlineLoggingCommand.java | 61 +++++++++++++++++++ 3 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java diff --git a/src/main/java/frc/robot/utils/logging/CommandUtil.java b/src/main/java/frc/robot/utils/logging/CommandUtil.java index 414dff0e..f5cfcdd0 100644 --- a/src/main/java/frc/robot/utils/logging/CommandUtil.java +++ b/src/main/java/frc/robot/utils/logging/CommandUtil.java @@ -113,6 +113,10 @@ public static Command race(String sequenceName, Command... commands) { return new RaceLoggingCommand(sequenceName, commands); } + public static Command deadline(String sequenceName, Command deadline, Command... commands) { + return new ParallelDeadlineLoggingCommand(sequenceName, deadline, commands); + } + public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) { // Do not use streams due to efficiency LoggingCommand[] newCommands = new LoggingCommand[commands.length]; diff --git a/src/main/java/frc/robot/utils/logging/LoggingCommand.java b/src/main/java/frc/robot/utils/logging/LoggingCommand.java index 442790d9..337034f1 100644 --- a/src/main/java/frc/robot/utils/logging/LoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/LoggingCommand.java @@ -11,7 +11,7 @@ public abstract class LoggingCommand extends Command { private String namePrefix; private String commandName; - private final Command underlying; + private Command underlying; private String fullyQualifiedName; // Lazy initialized log entry to make sure we don't create it until it is needed (and command is scheduled) @@ -25,6 +25,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); 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..cb58f4df --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java @@ -0,0 +1,61 @@ +package frc.robot.utils.logging; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; + +import java.util.Arrays; +import java.util.List; + +public class ParallelDeadlineLoggingCommand extends LoggingCommand { + private static final String THIS_NAME = "-this"; + + private LoggingCommand deadlineLoggingCommand; + private List loggingCommands; + + /** + * 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) { + super(namePrefix, THIS_NAME); + // Since we can't initialize the deadlineGroup with empty commands, use the special constructor to pass in the + // underlying afterward + deadlineLoggingCommand = CommandUtil.wrapForLogging(namePrefix, deadline)[0]; + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); + this.loggingCommands = Arrays.asList(wrapped); + setUnderlying(new ParallelDeadlineGroup(deadlineLoggingCommand, wrapped)); + } + + public final void addCommands(Command... commands) { + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); + ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); + loggingCommands.addAll(Arrays.asList(wrapped)); + } + + @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(); + } + + private void setChildrenPrefix(String prefix) { + // Recursively change the prefix for all child commands + for (LoggingCommand loggingCommand : loggingCommands) { + loggingCommand.setNamePrefix(prefix); + } + } +} From 22a61a7142875efee7a861cbdc08deaea2a8f762 Mon Sep 17 00:00:00 2001 From: ohad Date: Sat, 9 Mar 2024 17:12:33 -0500 Subject: [PATCH 4/5] Add deadline group, fix bug in 4+ deep commands, add superclass for groups --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 8 +++ .../sequences/StartIntakeAndFeeder.java | 44 ++++++------ .../frc/robot/utils/logging/CommandUtil.java | 8 +-- .../utils/logging/GroupLoggingCommand.java | 69 +++++++++++++++++++ .../robot/utils/logging/LoggingCommand.java | 16 ++++- .../ParallelDeadlineLoggingCommand.java | 46 ++++--------- .../utils/logging/ParallelLoggingCommand.java | 39 ++--------- .../utils/logging/RaceLoggingCommand.java | 39 ++--------- .../logging/SequentialLoggingCommand.java | 44 ++---------- 10 files changed, 148 insertions(+), 167 deletions(-) create mode 100644 src/main/java/frc/robot/utils/logging/GroupLoggingCommand.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c043ffd5..f73011b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,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; @@ -84,6 +85,7 @@ public void teleopInit() { } CommandUtil.logged(new RaiseDeployer(robotContainer.getDeployer())).schedule(); CommandUtil.logged(new ResetRamp(robotContainer.getRamp())).schedule(); + new StartIntakeAndFeeder(robotContainer.getFeeder(), robotContainer.getIntake(), robotContainer.getDeployer(), robotContainer.getRamp()).schedule(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 879433cf..2fe4e63d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -289,6 +289,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. * @@ -302,4 +306,8 @@ public static boolean isRedAlliance() { public AutoChooser getAutoChooser() { return autoChooser; } + + 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 f5cfcdd0..f375d170 100644 --- a/src/main/java/frc/robot/utils/logging/CommandUtil.java +++ b/src/main/java/frc/robot/utils/logging/CommandUtil.java @@ -113,10 +113,6 @@ public static Command race(String sequenceName, Command... commands) { return new RaceLoggingCommand(sequenceName, commands); } - public static Command deadline(String sequenceName, Command deadline, Command... commands) { - return new ParallelDeadlineLoggingCommand(sequenceName, deadline, commands); - } - public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) { // Do not use streams due to efficiency LoggingCommand[] newCommands = new LoggingCommand[commands.length]; @@ -128,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 337034f1..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; + // 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 @@ -100,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); } @@ -129,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 index cb58f4df..105a4db5 100644 --- a/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java @@ -1,61 +1,41 @@ package frc.robot.utils.logging; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import java.util.Arrays; -import java.util.List; - -public class ParallelDeadlineLoggingCommand extends LoggingCommand { - private static final String THIS_NAME = "-this"; +public class ParallelDeadlineLoggingCommand extends GroupLoggingCommand { private LoggingCommand deadlineLoggingCommand; - private List loggingCommands; /** * 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) + * @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) { - super(namePrefix, THIS_NAME); // 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); - this.loggingCommands = Arrays.asList(wrapped); setUnderlying(new ParallelDeadlineGroup(deadlineLoggingCommand, wrapped)); + addLoggingCommands(Arrays.asList(wrapped)); } public final void addCommands(Command... commands) { LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); - ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); - loggingCommands.addAll(Arrays.asList(wrapped)); - } - - @Override - public void setName(String name) { - // Do not change the logging name for this command (it is fixed) - getUnderlying().setName(name); + ((ParallelDeadlineGroup) getUnderlying()).addCommands(wrapped); + addLoggingCommands(Arrays.asList(wrapped)); } @Override - public void setNamePrefix(String prefix) { - super.setNamePrefix(prefix); - setChildrenPrefix(prefix); - } - - @Override - public String toString() { - return getFullyQualifiedName(); - } - - private void setChildrenPrefix(String prefix) { - // Recursively change the prefix for all child commands - for (LoggingCommand loggingCommand : loggingCommands) { - loggingCommand.setNamePrefix(prefix); - } + 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 af7e4ccf..7c5c7ebf 100644 --- a/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/ParallelLoggingCommand.java @@ -4,13 +4,8 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import java.util.Arrays; -import java.util.List; - -public class ParallelLoggingCommand extends LoggingCommand { - private static final String THIS_NAME = "-this"; - - private List loggingCommands; +public class ParallelLoggingCommand extends GroupLoggingCommand { /** * Constructor for parallel command group. * @@ -18,39 +13,17 @@ public class ParallelLoggingCommand extends LoggingCommand { * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK) */ public ParallelLoggingCommand(String namePrefix, Command... commands) { - super(namePrefix, THIS_NAME, new ParallelCommandGroup()); + // Call super with an empty group, populate children afterward + super(namePrefix, "(Parallel)", new ParallelCommandGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); - this.loggingCommands = Arrays.asList(wrapped); + addLoggingCommands(Arrays.asList(wrapped)); } public final void addCommands(Command... commands) { LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); ((ParallelCommandGroup) getUnderlying()).addCommands(wrapped); - loggingCommands.addAll(Arrays.asList(wrapped)); - } - - @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(); - } - - private void setChildrenPrefix(String prefix) { - // Recursively change the prefix for all child commands - for (LoggingCommand loggingCommand : loggingCommands) { - loggingCommand.setNamePrefix(prefix); - } + 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 0a6c46f9..21ff8c89 100644 --- a/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/RaceLoggingCommand.java @@ -4,13 +4,8 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import java.util.Arrays; -import java.util.List; - -public class RaceLoggingCommand extends LoggingCommand { - private static final String THIS_NAME = "-this"; - - private List loggingCommands; +public class RaceLoggingCommand extends GroupLoggingCommand { /** * Constructor for race command group. * @@ -18,39 +13,17 @@ public class RaceLoggingCommand extends LoggingCommand { * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK) */ public RaceLoggingCommand(String namePrefix, Command... commands) { - super(namePrefix, THIS_NAME, new ParallelRaceGroup()); + // Call super with an empty group, populate children afterward + super(namePrefix, "(Race)", new ParallelRaceGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped); - this.loggingCommands = Arrays.asList(wrapped); + addLoggingCommands(Arrays.asList(wrapped)); } public final void addCommands(Command... commands) { LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); ((ParallelRaceGroup) getUnderlying()).addCommands(wrapped); - loggingCommands.addAll(Arrays.asList(wrapped)); - } - - @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(); - } - - private void setChildrenPrefix(String prefix) { - // Recursively change the prefix for all child commands - for (LoggingCommand loggingCommand : loggingCommands) { - loggingCommand.setNamePrefix(prefix); - } + 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 98a4cdb8..983d6c79 100644 --- a/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java +++ b/src/main/java/frc/robot/utils/logging/SequentialLoggingCommand.java @@ -4,13 +4,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import java.util.Arrays; -import java.util.List; - -public class SequentialLoggingCommand extends LoggingCommand { - private static final String THIS_NAME = "-this"; - - private final List loggingCommands; +public class SequentialLoggingCommand extends GroupLoggingCommand { /** * Constructor for sequential command group. * @@ -18,44 +13,17 @@ public class SequentialLoggingCommand extends LoggingCommand { * @param commands the sub commands for this group (either regular commands or LoggingCommand are OK) */ public SequentialLoggingCommand(String namePrefix, Command... commands) { - super(namePrefix, THIS_NAME, new SequentialCommandGroup()); + // Call super with an empty group, populate children afterward + super(namePrefix, "(Sequence)", new SequentialCommandGroup()); + LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands); ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped); - this.loggingCommands = Arrays.asList(wrapped); + addLoggingCommands(Arrays.asList(wrapped)); } public final void addCommands(Command... commands) { LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands); ((SequentialCommandGroup) getUnderlying()).addCommands(wrapped); - loggingCommands.addAll(Arrays.asList(wrapped)); - } - - @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(); - } - - // For testing - public List getLoggingCommands() { - return loggingCommands; - } - - private void setChildrenPrefix(String prefix) { - // Recursively change the prefix for all child commands - for (LoggingCommand loggingCommand : loggingCommands) { - loggingCommand.setNamePrefix(prefix); - } + addLoggingCommands(Arrays.asList(wrapped)); } } From f1bebc9f169e5e71dc55348d0f1f44fc6828b5e3 Mon Sep 17 00:00:00 2001 From: ohad Date: Mon, 11 Mar 2024 21:52:51 -0400 Subject: [PATCH 5/5] Merge from main --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 136e7186..ca5c8309 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,7 +86,6 @@ public void teleopInit() { } CommandUtil.logged(new RaiseDeployer(robotContainer.getDeployer())).schedule(); CommandUtil.parallel("Reset Climber and Ramp",new teleOPinitReset(robotContainer.getRamp(), robotContainer.getClimber())).schedule(); - new StartIntakeAndFeeder(robotContainer.getFeeder(), robotContainer.getIntake(), robotContainer.getDeployer(), robotContainer.getRamp()).schedule(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 867c2830..5f71b6c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -332,10 +332,6 @@ public AutoChooser getAutoChooser() { return autoChooser; } - public Feeder getFeeder() { - return feeder; - } - public IntakeSubsystem getIntake() { return intake; }