Skip to content

Commit 79f2ef5

Browse files
armadillozarmadilloz
authored
Extend logging utility (#137)
* Enable constructor for logging command groups to take regular commands and wrap them * Add comments * Add deadline group * Add deadline group, fix bug in 4+ deep commands, add superclass for groups * Merge from main
1 parent d294f09 commit 79f2ef5

10 files changed

Lines changed: 255 additions & 127 deletions

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import frc.robot.commands.drivetrain.ResetGyro;
1717
import frc.robot.commands.drivetrain.WheelAlign;
1818
import frc.robot.commands.ramp.ResetRamp;
19+
import frc.robot.commands.sequences.StartIntakeAndFeeder;
1920
import frc.robot.constants.Constants;
2021
import frc.robot.utils.TimeoutCounter;
2122
import frc.robot.utils.diag.Diagnostics;

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -314,6 +314,10 @@ public Command getAutoCommand() {
314314
return autoChooser.getAutoCommand();
315315
}
316316

317+
public Feeder getFeeder() {
318+
return feeder;
319+
}
320+
317321
/**
318322
* Returns a boolean based on the current alliance color assigned by the FMS.
319323
*
@@ -328,7 +332,7 @@ public AutoChooser getAutoChooser() {
328332
return autoChooser;
329333
}
330334

331-
public Feeder getFeeder() {
332-
return feeder;
335+
public IntakeSubsystem getIntake() {
336+
return intake;
333337
}
334338
}

‎src/main/java/frc/robot/commands/sequences/StartIntakeAndFeeder.java‎

Lines changed: 24 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,43 +1,47 @@
11
package frc.robot.commands.sequences;
22

3-
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
4-
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
5-
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
63
import edu.wpi.first.wpilibj2.command.WaitCommand;
74
import frc.robot.commands.deployer.LowerDeployer;
85
import frc.robot.commands.deployer.RaiseDeployer;
6+
import frc.robot.commands.feeder.FeederBackDrive;
97
import frc.robot.commands.feeder.StartFeeder;
108
import frc.robot.commands.intake.StartIntake;
11-
import frc.robot.commands.feeder.FeederBackDrive;
129
import frc.robot.commands.ramp.RampMoveAndWait;
1310
import frc.robot.constants.GameConstants;
1411
import frc.robot.subsystems.Deployer;
1512
import frc.robot.subsystems.Feeder;
1613
import frc.robot.subsystems.IntakeSubsystem;
1714
import frc.robot.subsystems.Ramp;
15+
import frc.robot.utils.logging.CommandUtil;
16+
import frc.robot.utils.logging.SequentialLoggingCommand;
1817

1918
/**
2019
* Sequence to start intaking, this takes care of the lowering/raising the deployer,
2120
* as well as starting/stopping the intake and feeder.
2221
*/
23-
public class StartIntakeAndFeeder extends SequentialCommandGroup{
22+
public class StartIntakeAndFeeder extends SequentialLoggingCommand {
23+
@Override
24+
public void execute() {
25+
super.execute();
26+
}
27+
2428
public StartIntakeAndFeeder(Feeder feeder, IntakeSubsystem intake, Deployer deployer, Ramp ramp) {
25-
addCommands(
26-
new ParallelCommandGroup(
27-
new LowerDeployer(deployer),
28-
new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
29-
),
30-
new ParallelRaceGroup(
31-
new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
32-
new StartFeeder(feeder)
33-
),
34-
new ParallelCommandGroup(
35-
new RaiseDeployer(deployer),
36-
new SequentialCommandGroup(
37-
new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
38-
new FeederBackDrive(feeder)
29+
super("StartIntakeAndFeeder",
30+
CommandUtil.parallel("First",
31+
new LowerDeployer(deployer),
32+
new RampMoveAndWait(ramp, () -> GameConstants.RAMP_POS_STOW)
33+
),
34+
CommandUtil.race("second",
35+
new StartIntake(intake, 10), //intake stops by ParallelRaceGroup when note in feeder
36+
new StartFeeder(feeder)
37+
),
38+
CommandUtil.parallel("third",
39+
new RaiseDeployer(deployer),
40+
CommandUtil.sequence("Fourth",
41+
new WaitCommand(GameConstants.FEEDER_WAIT_TIME_BEFORE_BACKDRIVE),
42+
new FeederBackDrive(feeder)
43+
)
3944
)
40-
)
4145
);
4246
}
4347
}

‎src/main/java/frc/robot/utils/logging/CommandUtil.java‎

Lines changed: 20 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,18 @@
3232
* Command pickUp = CommandUtil.sequence("ArmSequence", new OpenGripper(), new RaiseArm(), new CloseGripper());
3333
* Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
3434
* </pre>
35+
*
36+
* and also for nesting command groups that are custom-created:
37+
* (Note the "extends SequentialLoggingCommandGroup" - DO NOT extend regular SequentialCommandGroup or this will not work)
38+
* <pre>
39+
* class PickupGroup extends SequentialLoggingCommandGroup {
40+
* public PickupGroup() {
41+
* super("Pickup", new OpenGripper(), new RaiseArm(), new CloseGripper());
42+
* }
43+
* }
44+
* Command pickUp = new PickupGroup();
45+
* Command pickupAndDrive = CommandUtil.parallel("WalkWhileChewingGum", pickup, new Drive());
46+
* </pre>
3547
*/
3648
public class CommandUtil {
3749
public static final String COMMAND_PREFIX = "Commands";
@@ -52,6 +64,8 @@ public static Command logged(Command command) {
5264
/**
5365
* Make a logging command from the given command, with a custom name hierarchy.
5466
* The logging command will be placed within the given name prefix naming hierarchy instead of at the root.
67+
* @param namePrefix the nesting hierarchy prefix for the command. If null the command is going to log at the
68+
* root level. If not null it will be nested in this namespace.
5569
* @param command the command to wrap
5670
* @return a {@link LoggingCommand} that wraps the given command
5771
*/
@@ -76,30 +90,27 @@ public static Command runOnce(String name, Runnable action, Subsystem... require
7690
/**
7791
* Return a loggable {@link SequentialCommandGroup} that executes the given commands sequentially. The given commands
7892
* can be any command or command group - they will be converted to loggable commands implicitly.
79-
* @param sequenceName the name of the resaulting command group
93+
* @param sequenceName the name of the resulting command group
8094
* @param commands the commands to put in the group
8195
* @return the created loggable command group
8296
*/
8397
public static Command sequence(String sequenceName, Command... commands) {
84-
LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
85-
return new SequentialLoggingCommand(sequenceName, loggingCommands);
98+
return new SequentialLoggingCommand(sequenceName, commands);
8699
}
87100

88101
/**
89102
* Return a loggable {@link ParallelCommandGroup} that executes the given commands in parallel. The given commands
90103
* can be any command or command group - they will be converted to loggable commands implicitly.
91-
* @param sequenceName the name of the resaulting command group
104+
* @param sequenceName the name of the resulting command group
92105
* @param commands the commands to put in the group
93106
* @return the created loggable command group
94107
*/
95108
public static Command parallel(String sequenceName, Command... commands) {
96-
LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
97-
return new ParallelLoggingCommand(sequenceName, loggingCommands);
109+
return new ParallelLoggingCommand(sequenceName, commands);
98110
}
99111

100112
public static Command race(String sequenceName, Command... commands) {
101-
LoggingCommand[] loggingCommands = wrapForLogging(sequenceName, commands);
102-
return new RaceLoggingCommand(sequenceName, loggingCommands);
113+
return new RaceLoggingCommand(sequenceName, commands);
103114
}
104115

105116
public static LoggingCommand[] wrapForLogging(String prefix, Command... commands) {
@@ -113,9 +124,7 @@ public static LoggingCommand[] wrapForLogging(String prefix, Command... commands
113124

114125
private static LoggingCommand wrapForLogging(String prefix, Command command) {
115126
if (command instanceof LoggingCommand loggingCommand) {
116-
// change the prefix to include the current new parent
117-
String childPrefix = prefix + CommandUtil.NAME_SEPARATOR + loggingCommand.getNamePrefix();
118-
loggingCommand.setNamePrefix(childPrefix);
127+
loggingCommand.appendNamePrefix(prefix);
119128
return loggingCommand;
120129
} else {
121130
// New command located in the given sequence root

‎src/main/java/frc/robot/utils/logging/GroupLoggingCommand.java‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
package frc.robot.utils.logging;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
5+
import java.util.ArrayList;
6+
import java.util.List;
7+
8+
/**
9+
* Base class for logged grouped commands (parallel, race, sequential...)
10+
*/
11+
public abstract class GroupLoggingCommand extends LoggingCommand {
12+
// A copy of the children (since we can't access them from the regular groups)
13+
private final List<LoggingCommand> childLoggingCommands;
14+
15+
/**
16+
* Constructor for logged group command.
17+
*
18+
* @param namePrefix the prefix for the name (comes in front of hte group name)
19+
* @param underlying the group command that is wrapped by this command
20+
*/
21+
public GroupLoggingCommand(String namePrefix, String commandName, Command underlying) {
22+
super(namePrefix, commandName, underlying);
23+
childLoggingCommands = new ArrayList<>();
24+
}
25+
26+
/**
27+
* A special constructor to allow for the creation of the command when we can't create the underlying up front.
28+
* It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
29+
*/
30+
protected GroupLoggingCommand(String namePrefix, String commandName) {
31+
super(namePrefix, commandName);
32+
childLoggingCommands = new ArrayList<>();
33+
}
34+
35+
public final void addLoggingCommands(List<LoggingCommand> commands) {
36+
childLoggingCommands.addAll(commands);
37+
}
38+
39+
@Override
40+
public void setName(String name) {
41+
// Do not change the logging name for this command (it is fixed)
42+
getUnderlying().setName(name);
43+
}
44+
45+
@Override
46+
public void appendNamePrefix(String prefix) {
47+
// Change the name for this command
48+
super.appendNamePrefix(prefix);
49+
// Change the name for the children
50+
appendChildrenPrefix(prefix);
51+
}
52+
53+
@Override
54+
public String toString() {
55+
return getFullyQualifiedName();
56+
}
57+
58+
// For testing
59+
public List<LoggingCommand> getChildLoggingCommands() {
60+
return childLoggingCommands;
61+
}
62+
63+
private void appendChildrenPrefix(String prefix) {
64+
// Recursively change the prefix for all child commands
65+
for (LoggingCommand loggingCommand : childLoggingCommands) {
66+
loggingCommand.appendNamePrefix(prefix);
67+
}
68+
}
69+
}

‎src/main/java/frc/robot/utils/logging/LoggingCommand.java‎

Lines changed: 31 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,14 @@
99
import java.util.Set;
1010

1111
public abstract class LoggingCommand extends Command {
12+
// Prefix for the command name (appended ahead of the command name) - this can be appended to when the command gets
13+
// embedded at another command through appendNamePrefix
1214
private String namePrefix;
15+
// Private name for the command (last in the hierarchy for this command) - this can be changed through setName
1316
private String commandName;
14-
private final Command underlying;
15-
17+
// The delegate command where we refer all calls
18+
private Command underlying;
19+
// The name as it would be used in the logs
1620
private String fullyQualifiedName;
1721
// Lazy initialized log entry to make sure we don't create it until it is needed (and command is scheduled)
1822
// otherwise we get an empty line in the log visualization tool
@@ -25,6 +29,23 @@ public LoggingCommand(String namePrefix, String commandName, Command underlying)
2529
resetLoggingName(namePrefix, commandName);
2630
}
2731

32+
/**
33+
* A special constructor to allow for the creation of the command when we can't create the underlying up front.
34+
* It is assumed that the {@link #setUnderlying(Command)} method is called immediately following the construction.
35+
*/
36+
protected LoggingCommand(String namePrefix, String commandName) {
37+
this.namePrefix = namePrefix;
38+
this.commandName = commandName;
39+
resetLoggingName(namePrefix, commandName);
40+
}
41+
42+
/**
43+
* Second phase of initialization - set the underlying command
44+
*/
45+
protected void setUnderlying(Command underlying) {
46+
this.underlying = underlying;
47+
}
48+
2849
@Override
2950
public void initialize() {
3051
if (Constants.ENABLE_LOGGING) getLoggingEntry().append(true);
@@ -83,8 +104,12 @@ public String getNamePrefix() {
83104
return namePrefix;
84105
}
85106

86-
public void setNamePrefix(String prefix) {
87-
this.namePrefix = prefix;
107+
/**
108+
* Add a new prefix in front of the current one
109+
* @param prefix the new prefix
110+
*/
111+
public void appendNamePrefix(String prefix) {
112+
this.namePrefix = prefix + CommandUtil.NAME_SEPARATOR + namePrefix;
88113
resetLoggingName(namePrefix, commandName);
89114
}
90115

@@ -112,3 +137,5 @@ private BooleanLogEntry getLoggingEntry() {
112137
return logEntry;
113138
}
114139
}
140+
141+

‎src/main/java/frc/robot/utils/logging/ParallelDeadlineLoggingCommand.java‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
package frc.robot.utils.logging;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
5+
6+
import java.util.Arrays;
7+
8+
public class ParallelDeadlineLoggingCommand extends GroupLoggingCommand {
9+
private LoggingCommand deadlineLoggingCommand;
10+
11+
/**
12+
* Constructor for parallel deadline command group.
13+
*
14+
* @param namePrefix the name for the group - this is where the sub-commands for this group will be nested in
15+
* @param deadline the deadline command - the one that when ends, stops other commands
16+
* @param commands the sub commands for this group (either regular commands or LoggingCommand are OK)
17+
*/
18+
public ParallelDeadlineLoggingCommand(String namePrefix, Command deadline, Command... commands) {
19+
// Since we can't initialize the deadlineGroup with empty commands, use the special constructor to pass in the
20+
// underlying afterward
21+
super(namePrefix, "(Deadline)");
22+
23+
deadlineLoggingCommand = CommandUtil.wrapForLogging(namePrefix, deadline)[0];
24+
LoggingCommand[] wrapped = CommandUtil.wrapForLogging(namePrefix, commands);
25+
setUnderlying(new ParallelDeadlineGroup(deadlineLoggingCommand, wrapped));
26+
addLoggingCommands(Arrays.asList(wrapped));
27+
}
28+
29+
public final void addCommands(Command... commands) {
30+
LoggingCommand[] wrapped = CommandUtil.wrapForLogging(getNamePrefix(), commands);
31+
((ParallelDeadlineGroup) getUnderlying()).addCommands(wrapped);
32+
addLoggingCommands(Arrays.asList(wrapped));
33+
}
34+
35+
@Override
36+
public void appendNamePrefix(String prefix) {
37+
super.appendNamePrefix(prefix);
38+
// Change the name for the deadline
39+
deadlineLoggingCommand.appendNamePrefix(prefix);
40+
}
41+
}

0 commit comments

Comments
 (0)