[commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392)

Moves all CommandBase functionality into Command and deprecates CommandBase for removal.
Moves all SubsystemBase functionality into Subsystem and deprecates SubsystemBase for removal.
Adds a function to CommandScheduler to remove all registered Subsystems.
This commit is contained in:
Ryan Blue
2023-07-14 01:12:01 -04:00
committed by GitHub
parent 7ac932996a
commit aaea85ff16
176 changed files with 887 additions and 910 deletions

View File

@@ -7,6 +7,10 @@ package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.util.function.BooleanConsumer;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.util.HashSet;
import java.util.Set;
import java.util.function.BooleanSupplier;
@@ -20,12 +24,19 @@ import java.util.function.BooleanSupplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public interface Command {
public abstract class Command implements Sendable {
protected Set<Subsystem> m_requirements = new HashSet<>();
protected Command() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
}
/** The initial subroutine of a command. Called once when the command is initially scheduled. */
default void initialize() {}
public void initialize() {}
/** The main body of a command. Called repeatedly while the command is scheduled. */
default void execute() {}
public void execute() {}
/**
* The action to take when the command ends. Called when either the command finishes normally, or
@@ -36,7 +47,7 @@ public interface Command {
*
* @param interrupted whether the command was interrupted/canceled
*/
default void end(boolean interrupted) {}
public void end(boolean interrupted) {}
/**
* Whether the command has finished. Once a command finishes, the scheduler will call its end()
@@ -44,7 +55,7 @@ public interface Command {
*
* @return whether the command has finished.
*/
default boolean isFinished() {
public boolean isFinished() {
return false;
}
@@ -60,7 +71,63 @@ public interface Command {
* @return the set of subsystems that are required
* @see InterruptionBehavior
*/
Set<Subsystem> getRequirements();
public Set<Subsystem> getRequirements() {
return m_requirements;
}
/**
* Adds the specified subsystems to the requirements of the command. The scheduler will prevent
* two commands that require the same subsystem from being scheduled simultaneously.
*
* <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
* this method should normally be called from the command's constructor.
*
* @param requirements the requirements to add
*/
public final void addRequirements(Subsystem... requirements) {
for (Subsystem requirement : requirements) {
m_requirements.add(requireNonNullParam(requirement, "requirement", "addRequirements"));
}
}
/**
* Gets the name of this Command.
*
* <p>By default, the simple class name is used. This can be changed with {@link
* #setName(String)}.
*
* @return The display name of the Command
*/
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Command.
*
* @param name The display name of the Command.
*/
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Command.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Command.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Decorates this command with a timeout. If the specified timeout is exceeded before the command
@@ -75,7 +142,7 @@ public interface Command {
* @param seconds the timeout duration
* @return the command with the timeout added
*/
default ParallelRaceGroup withTimeout(double seconds) {
public ParallelRaceGroup withTimeout(double seconds) {
return raceWith(new WaitCommand(seconds));
}
@@ -93,7 +160,7 @@ public interface Command {
* @return the command with the interrupt condition added
* @see #onlyWhile(BooleanSupplier)
*/
default ParallelRaceGroup until(BooleanSupplier condition) {
public ParallelRaceGroup until(BooleanSupplier condition) {
return raceWith(new WaitUntilCommand(condition));
}
@@ -111,7 +178,7 @@ public interface Command {
* @return the command with the run condition added
* @see #until(BooleanSupplier)
*/
default ParallelRaceGroup onlyWhile(BooleanSupplier condition) {
public ParallelRaceGroup onlyWhile(BooleanSupplier condition) {
return until(() -> !condition.getAsBoolean());
}
@@ -128,7 +195,7 @@ public interface Command {
* @param requirements the required subsystems
* @return the decorated command
*/
default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
public SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
return beforeStarting(new InstantCommand(toRun, requirements));
}
@@ -144,7 +211,7 @@ public interface Command {
* @param before the command to run before this one
* @return the decorated command
*/
default SequentialCommandGroup beforeStarting(Command before) {
public SequentialCommandGroup beforeStarting(Command before) {
return new SequentialCommandGroup(before, this);
}
@@ -161,7 +228,7 @@ public interface Command {
* @param requirements the required subsystems
* @return the decorated command
*/
default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
public SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
return andThen(new InstantCommand(toRun, requirements));
}
@@ -178,7 +245,7 @@ public interface Command {
* @param next the commands to run next
* @return the decorated command
*/
default SequentialCommandGroup andThen(Command... next) {
public SequentialCommandGroup andThen(Command... next) {
SequentialCommandGroup group = new SequentialCommandGroup(this);
group.addCommands(next);
return group;
@@ -198,7 +265,7 @@ public interface Command {
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelDeadlineGroup deadlineWith(Command... parallel) {
public ParallelDeadlineGroup deadlineWith(Command... parallel) {
return new ParallelDeadlineGroup(this, parallel);
}
@@ -216,7 +283,7 @@ public interface Command {
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelCommandGroup alongWith(Command... parallel) {
public ParallelCommandGroup alongWith(Command... parallel) {
ParallelCommandGroup group = new ParallelCommandGroup(this);
group.addCommands(parallel);
return group;
@@ -236,7 +303,7 @@ public interface Command {
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelRaceGroup raceWith(Command... parallel) {
public ParallelRaceGroup raceWith(Command... parallel) {
ParallelRaceGroup group = new ParallelRaceGroup(this);
group.addCommands(parallel);
return group;
@@ -254,7 +321,7 @@ public interface Command {
*
* @return the decorated command
*/
default RepeatCommand repeatedly() {
public RepeatCommand repeatedly() {
return new RepeatCommand(this);
}
@@ -265,7 +332,7 @@ public interface Command {
*
* @return the decorated command
*/
default ProxyCommand asProxy() {
public ProxyCommand asProxy() {
return new ProxyCommand(this);
}
@@ -284,7 +351,7 @@ public interface Command {
* @return the decorated command
* @see #onlyIf(BooleanSupplier)
*/
default ConditionalCommand unless(BooleanSupplier condition) {
public ConditionalCommand unless(BooleanSupplier condition) {
return new ConditionalCommand(new InstantCommand(), this, condition);
}
@@ -303,7 +370,7 @@ public interface Command {
* @return the decorated command
* @see #unless(BooleanSupplier)
*/
default ConditionalCommand onlyIf(BooleanSupplier condition) {
public ConditionalCommand onlyIf(BooleanSupplier condition) {
return unless(() -> !condition.getAsBoolean());
}
@@ -313,7 +380,7 @@ public interface Command {
* @param doesRunWhenDisabled true to run when disabled.
* @return the decorated command
*/
default WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) {
public WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) {
return new WrapperCommand(this) {
@Override
public boolean runsWhenDisabled() {
@@ -328,7 +395,7 @@ public interface Command {
* @param interruptBehavior the desired interrupt behavior
* @return the decorated command
*/
default WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
public WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
return new WrapperCommand(this) {
@Override
public InterruptionBehavior getInterruptionBehavior() {
@@ -345,7 +412,7 @@ public interface Command {
* interrupted.
* @return the decorated command
*/
default WrapperCommand finallyDo(BooleanConsumer end) {
public WrapperCommand finallyDo(BooleanConsumer end) {
requireNonNullParam(end, "end", "Command.finallyDo()");
return new WrapperCommand(this) {
@Override
@@ -363,7 +430,7 @@ public interface Command {
* @param handler a lambda to run when the command is interrupted
* @return the decorated command
*/
default WrapperCommand handleInterrupt(Runnable handler) {
public WrapperCommand handleInterrupt(Runnable handler) {
requireNonNullParam(handler, "handler", "Command.handleInterrupt()");
return finallyDo(
interrupted -> {
@@ -374,7 +441,7 @@ public interface Command {
}
/** Schedules this command. */
default void schedule() {
public void schedule() {
CommandScheduler.getInstance().schedule(this);
}
@@ -384,7 +451,7 @@ public interface Command {
*
* @see CommandScheduler#cancel(Command...)
*/
default void cancel() {
public void cancel() {
CommandScheduler.getInstance().cancel(this);
}
@@ -394,7 +461,7 @@ public interface Command {
*
* @return Whether the command is scheduled.
*/
default boolean isScheduled() {
public boolean isScheduled() {
return CommandScheduler.getInstance().isScheduled(this);
}
@@ -404,7 +471,7 @@ public interface Command {
* @param requirement the subsystem to inquire about
* @return whether the subsystem is required
*/
default boolean hasRequirement(Subsystem requirement) {
public boolean hasRequirement(Subsystem requirement) {
return getRequirements().contains(requirement);
}
@@ -414,7 +481,7 @@ public interface Command {
* @return a variant of {@link InterruptionBehavior}, defaulting to {@link
* InterruptionBehavior#kCancelSelf kCancelSelf}.
*/
default InterruptionBehavior getInterruptionBehavior() {
public InterruptionBehavior getInterruptionBehavior() {
return InterruptionBehavior.kCancelSelf;
}
@@ -424,43 +491,52 @@ public interface Command {
*
* @return whether the command should run when the robot is disabled
*/
default boolean runsWhenDisabled() {
public boolean runsWhenDisabled() {
return false;
}
/**
* Gets the name of this Command. Defaults to the simple class name if not overridden.
*
* @return The display name of the Command
*/
default String getName() {
return this.getClass().getSimpleName();
}
/**
* Sets the name of this Command. Nullop if not overridden.
*
* @param name The display name of the Command.
*/
default void setName(String name) {}
/**
* Decorates this Command with a name.
*
* @param name name
* @return the decorated Command
*/
default WrapperCommand withName(String name) {
public WrapperCommand withName(String name) {
WrapperCommand wrapper = new WrapperCommand(Command.this) {};
wrapper.setName(name);
return wrapper;
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Command");
builder.addStringProperty(".name", this::getName, null);
builder.addBooleanProperty(
"running",
this::isScheduled,
value -> {
if (value) {
if (!isScheduled()) {
schedule();
}
} else {
if (isScheduled()) {
cancel();
}
}
});
builder.addBooleanProperty(
".isParented", () -> CommandScheduler.getInstance().isComposed(this), null);
builder.addStringProperty(
"interruptBehavior", () -> getInterruptionBehavior().toString(), null);
builder.addBooleanProperty("runsWhenDisabled", this::runsWhenDisabled, null);
}
/**
* An enum describing the command's behavior when another command with a shared requirement is
* scheduled.
*/
enum InterruptionBehavior {
public enum InterruptionBehavior {
/**
* This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
* scheduled normally.

View File

@@ -4,107 +4,16 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.util.HashSet;
import java.util.Set;
/**
* A {@link Sendable} base class for {@link Command}s.
*
* <p>This class is provided by the NewCommands VendorDep
*
* @deprecated All functionality provided by {@link CommandBase} has been merged into {@link
* Command}. Use {@link Command} instead.
*/
public abstract class CommandBase implements Sendable, Command {
protected Set<Subsystem> m_requirements = new HashSet<>();
protected CommandBase() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
}
/**
* Adds the specified subsystems to the requirements of the command. The scheduler will prevent
* two commands that require the same subsystem from being scheduled simultaneously.
*
* <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
* this method should normally be called from the command's constructor.
*
* @param requirements the requirements to add
*/
public final void addRequirements(Subsystem... requirements) {
for (Subsystem requirement : requirements) {
m_requirements.add(requireNonNullParam(requirement, "requirement", "addRequirements"));
}
}
@Override
public Set<Subsystem> getRequirements() {
return m_requirements;
}
@Override
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Command.
*
* @param name name
*/
@Override
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Command.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Command.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
*
* @param builder the builder used to construct this sendable
*/
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Command");
builder.addStringProperty(".name", this::getName, null);
builder.addBooleanProperty(
"running",
this::isScheduled,
value -> {
if (value) {
if (!isScheduled()) {
schedule();
}
} else {
if (isScheduled()) {
cancel();
}
}
});
builder.addBooleanProperty(
".isParented", () -> CommandScheduler.getInstance().isComposed(this), null);
builder.addStringProperty(
"interruptBehavior", () -> getInterruptionBehavior().toString(), null);
builder.addBooleanProperty("runsWhenDisabled", this::runsWhenDisabled, null);
}
}
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("PMD.AbstractClassWithoutAnyMethod")
public abstract class CommandBase extends Command {}

View File

@@ -362,6 +362,15 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_subsystems.keySet().removeAll(Set.of(subsystems));
}
/**
* Un-registers all registered Subsystems with the scheduler. All currently registered subsystems
* will no longer have their periodic block called, and will not have their default command
* scheduled.
*/
public void unregisterAllSubsystems() {
m_subsystems.clear();
}
/**
* Sets the default command for a subsystem. Registers that subsystem if it is not already
* registered. Default commands will run whenever there is no other command currently scheduled

View File

@@ -21,7 +21,7 @@ public final class Commands {
*
* @return the command
*/
public static CommandBase none() {
public static Command none() {
return new InstantCommand();
}
@@ -35,7 +35,7 @@ public final class Commands {
* @return the command
* @see InstantCommand
*/
public static CommandBase runOnce(Runnable action, Subsystem... requirements) {
public static Command runOnce(Runnable action, Subsystem... requirements) {
return new InstantCommand(action, requirements);
}
@@ -47,7 +47,7 @@ public final class Commands {
* @return the command
* @see RunCommand
*/
public static CommandBase run(Runnable action, Subsystem... requirements) {
public static Command run(Runnable action, Subsystem... requirements) {
return new RunCommand(action, requirements);
}
@@ -61,7 +61,7 @@ public final class Commands {
* @return the command
* @see StartEndCommand
*/
public static CommandBase startEnd(Runnable start, Runnable end, Subsystem... requirements) {
public static Command startEnd(Runnable start, Runnable end, Subsystem... requirements) {
return new StartEndCommand(start, end, requirements);
}
@@ -74,7 +74,7 @@ public final class Commands {
* @param requirements subsystems the action requires
* @return the command
*/
public static CommandBase runEnd(Runnable run, Runnable end, Subsystem... requirements) {
public static Command runEnd(Runnable run, Runnable end, Subsystem... requirements) {
requireNonNullParam(end, "end", "Command.runEnd");
return new FunctionalCommand(
() -> {}, run, interrupted -> end.run(), () -> false, requirements);
@@ -87,7 +87,7 @@ public final class Commands {
* @return the command
* @see PrintCommand
*/
public static CommandBase print(String message) {
public static Command print(String message) {
return new PrintCommand(message);
}
@@ -100,7 +100,7 @@ public final class Commands {
* @return the command
* @see WaitCommand
*/
public static CommandBase waitSeconds(double seconds) {
public static Command waitSeconds(double seconds) {
return new WaitCommand(seconds);
}
@@ -111,7 +111,7 @@ public final class Commands {
* @return the command
* @see WaitUntilCommand
*/
public static CommandBase waitUntil(BooleanSupplier condition) {
public static Command waitUntil(BooleanSupplier condition) {
return new WaitUntilCommand(condition);
}
@@ -126,7 +126,7 @@ public final class Commands {
* @return the command
* @see ConditionalCommand
*/
public static CommandBase either(Command onTrue, Command onFalse, BooleanSupplier selector) {
public static Command either(Command onTrue, Command onFalse, BooleanSupplier selector) {
return new ConditionalCommand(onTrue, onFalse, selector);
}
@@ -138,7 +138,7 @@ public final class Commands {
* @return the command
* @see SelectCommand
*/
public static CommandBase select(Map<Object, Command> commands, Supplier<Object> selector) {
public static Command select(Map<Object, Command> commands, Supplier<Object> selector) {
return new SelectCommand(commands, selector);
}
@@ -149,7 +149,7 @@ public final class Commands {
* @return the command group
* @see SequentialCommandGroup
*/
public static CommandBase sequence(Command... commands) {
public static Command sequence(Command... commands) {
return new SequentialCommandGroup(commands);
}
@@ -164,7 +164,7 @@ public final class Commands {
* @see SequentialCommandGroup
* @see Command#repeatedly()
*/
public static CommandBase repeatingSequence(Command... commands) {
public static Command repeatingSequence(Command... commands) {
return sequence(commands).repeatedly();
}
@@ -175,7 +175,7 @@ public final class Commands {
* @return the command
* @see ParallelCommandGroup
*/
public static CommandBase parallel(Command... commands) {
public static Command parallel(Command... commands) {
return new ParallelCommandGroup(commands);
}
@@ -187,7 +187,7 @@ public final class Commands {
* @return the command group
* @see ParallelRaceGroup
*/
public static CommandBase race(Command... commands) {
public static Command race(Command... commands) {
return new ParallelRaceGroup(commands);
}
@@ -200,7 +200,7 @@ public final class Commands {
* @return the command group
* @see ParallelDeadlineGroup
*/
public static CommandBase deadline(Command deadline, Command... commands) {
public static Command deadline(Command deadline, Command... commands) {
return new ParallelDeadlineGroup(deadline, commands);
}

View File

@@ -19,7 +19,7 @@ import java.util.function.BooleanSupplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ConditionalCommand extends CommandBase {
public class ConditionalCommand extends Command {
private final Command m_onTrue;
private final Command m_onFalse;
private final BooleanSupplier m_condition;

View File

@@ -17,7 +17,7 @@ import java.util.function.Consumer;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class FunctionalCommand extends CommandBase {
public class FunctionalCommand extends Command {
protected final Runnable m_onInit;
protected final Runnable m_onExecute;
protected final Consumer<Boolean> m_onEnd;

View File

@@ -38,7 +38,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class MecanumControllerCommand extends CommandBase {
public class MecanumControllerCommand extends Command {
private final Timer m_timer = new Timer();
private final boolean m_usePID;
private final Trajectory m_trajectory;

View File

@@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.Notifier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class NotifierCommand extends CommandBase {
public class NotifierCommand extends Command {
protected final Notifier m_notifier;
protected final double m_period;

View File

@@ -18,7 +18,7 @@ import java.util.function.DoubleSupplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class PIDCommand extends CommandBase {
public class PIDCommand extends Command {
protected final PIDController m_controller;
protected DoubleSupplier m_measurement;
protected DoubleSupplier m_setpoint;

View File

@@ -14,7 +14,7 @@ import edu.wpi.first.math.controller.PIDController;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class PIDSubsystem extends SubsystemBase {
public abstract class PIDSubsystem extends Subsystem {
protected final PIDController m_controller;
protected boolean m_enabled;

View File

@@ -17,7 +17,7 @@ import java.util.Map;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ParallelCommandGroup extends CommandBase {
public class ParallelCommandGroup extends Command {
// maps commands in this composition to whether they are still running
private final Map<Command, Boolean> m_commands = new HashMap<>();
private boolean m_runWhenDisabled = true;

View File

@@ -20,7 +20,7 @@ import java.util.Map;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ParallelDeadlineGroup extends CommandBase {
public class ParallelDeadlineGroup extends Command {
// maps commands in this composition to whether they are still running
private final Map<Command, Boolean> m_commands = new HashMap<>();
private boolean m_runWhenDisabled = true;

View File

@@ -18,7 +18,7 @@ import java.util.Set;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ParallelRaceGroup extends CommandBase {
public class ParallelRaceGroup extends Command {
private final Set<Command> m_commands = new HashSet<>();
private boolean m_runWhenDisabled = true;
private boolean m_finished = true;

View File

@@ -20,7 +20,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ProfiledPIDCommand extends CommandBase {
public class ProfiledPIDCommand extends Command {
protected final ProfiledPIDController m_controller;
protected DoubleSupplier m_measurement;
protected Supplier<State> m_goal;

View File

@@ -16,7 +16,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class ProfiledPIDSubsystem extends SubsystemBase {
public abstract class ProfiledPIDSubsystem extends Subsystem {
protected final ProfiledPIDController m_controller;
protected boolean m_enabled;

View File

@@ -15,7 +15,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ProxyCommand extends CommandBase {
public class ProxyCommand extends Command {
private final Supplier<Command> m_supplier;
private Command m_command;

View File

@@ -33,7 +33,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class RamseteCommand extends CommandBase {
public class RamseteCommand extends Command {
private final Timer m_timer = new Timer();
private final boolean m_usePID;
private final Trajectory m_trajectory;

View File

@@ -19,7 +19,7 @@ import edu.wpi.first.util.sendable.SendableBuilder;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class RepeatCommand extends CommandBase {
public class RepeatCommand extends Command {
protected final Command m_command;
private boolean m_ended;

View File

@@ -13,7 +13,7 @@ import java.util.Set;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class ScheduleCommand extends CommandBase {
public class ScheduleCommand extends Command {
private final Set<Command> m_toSchedule;
/**

View File

@@ -20,7 +20,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class SelectCommand extends CommandBase {
public class SelectCommand extends Command {
private final Map<Object, Command> m_commands;
private final Supplier<Object> m_selector;
private Command m_selectedCommand;

View File

@@ -17,7 +17,7 @@ import java.util.List;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class SequentialCommandGroup extends CommandBase {
public class SequentialCommandGroup extends Command {
private final List<Command> m_commands = new ArrayList<>();
private int m_currentCommandIndex = -1;
private boolean m_runWhenDisabled = true;

View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
/**
* A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
* framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
@@ -13,29 +17,34 @@ package edu.wpi.first.wpilibj2.command;
* their {@link Command#getRequirements()} method, and resources used within a subsystem should
* generally remain encapsulated and not be shared by other parts of the robot.
*
* <p>Subsystems must be registered with the scheduler with the {@link
* CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link
* Subsystem#periodic()} method to be called. It is recommended that this method be called from the
* constructor of users' Subsystem implementations. The {@link SubsystemBase} class offers a simple
* base for user implementations that handles this.
* <p>Subsystems are automatically registered with the default scheduler in order for the {@link
* Subsystem#periodic()} method to be called.
*
* <p>This class is provided by the NewCommands VendorDep
*/
public interface Subsystem {
public abstract class Subsystem implements Sendable {
/** Constructor. */
public Subsystem() {
String name = this.getClass().getSimpleName();
name = name.substring(name.lastIndexOf('.') + 1);
SendableRegistry.addLW(this, name, name);
CommandScheduler.getInstance().registerSubsystem(this);
}
/**
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
* subsystem-specific state that you don't want to offload to a {@link Command}. Teams should try
* to be consistent within their own codebases about which responsibilities will be handled by
* Commands, and which will be handled here.
*/
default void periodic() {}
public void periodic() {}
/**
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
* subsystem-specific state that needs to be maintained for simulations, such as for updating
* {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
*/
default void simulationPeriodic() {}
public void simulationPeriodic() {}
/**
* Sets the default {@link Command} of the subsystem. The default command will be automatically
@@ -46,7 +55,7 @@ public interface Subsystem {
*
* @param defaultCommand the default command to associate with this subsystem
*/
default void setDefaultCommand(Command defaultCommand) {
public void setDefaultCommand(Command defaultCommand) {
CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
}
@@ -54,7 +63,7 @@ public interface Subsystem {
* Removes the default command for the subsystem. This will not cancel the default command if it
* is currently running.
*/
default void removeDefaultCommand() {
public void removeDefaultCommand() {
CommandScheduler.getInstance().removeDefaultCommand(this);
}
@@ -64,7 +73,7 @@ public interface Subsystem {
*
* @return the default command associated with this subsystem
*/
default Command getDefaultCommand() {
public Command getDefaultCommand() {
return CommandScheduler.getInstance().getDefaultCommand(this);
}
@@ -74,15 +83,51 @@ public interface Subsystem {
*
* @return the scheduled command currently requiring this subsystem
*/
default Command getCurrentCommand() {
public Command getCurrentCommand() {
return CommandScheduler.getInstance().requiring(this);
}
/**
* Gets the name of this Subsystem.
*
* @return Name
*/
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Subsystem.
*
* @param name name
*/
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Subsystem.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Subsystem.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Registers this subsystem with the {@link CommandScheduler}, allowing its {@link
* Subsystem#periodic()} method to be called when the scheduler runs.
*/
default void register() {
public void register() {
CommandScheduler.getInstance().registerSubsystem(this);
}
@@ -93,7 +138,7 @@ public interface Subsystem {
* @return the command
* @see InstantCommand
*/
default CommandBase runOnce(Runnable action) {
public Command runOnce(Runnable action) {
return Commands.runOnce(action, this);
}
@@ -105,7 +150,7 @@ public interface Subsystem {
* @return the command
* @see RunCommand
*/
default CommandBase run(Runnable action) {
public Command run(Runnable action) {
return Commands.run(action, this);
}
@@ -118,7 +163,7 @@ public interface Subsystem {
* @return the command
* @see StartEndCommand
*/
default CommandBase startEnd(Runnable start, Runnable end) {
public Command startEnd(Runnable start, Runnable end) {
return Commands.startEnd(start, end, this);
}
@@ -130,7 +175,33 @@ public interface Subsystem {
* @param end the action to run on interrupt
* @return the command
*/
default CommandBase runEnd(Runnable run, Runnable end) {
public Command runEnd(Runnable run, Runnable end) {
return Commands.runEnd(run, end, this);
}
/**
* Associates a {@link Sendable} with this Subsystem. Also update the child's name.
*
* @param name name to give child
* @param child sendable
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
builder.addStringProperty(
".default",
() -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none",
null);
builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
builder.addStringProperty(
".command",
() -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none",
null);
}
}

View File

@@ -4,84 +4,15 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
/**
* A base for subsystems that handles registration in the constructor, and provides a more intuitive
* method for setting the default command.
*
* <p>This class is provided by the NewCommands VendorDep
*
* @deprecated All functionality provided by {@link SubsystemBase} has been merged into {@link
* Subsystem}. Use {@link Subsystem} instead.
*/
public abstract class SubsystemBase implements Subsystem, Sendable {
/** Constructor. */
public SubsystemBase() {
String name = this.getClass().getSimpleName();
name = name.substring(name.lastIndexOf('.') + 1);
SendableRegistry.addLW(this, name, name);
CommandScheduler.getInstance().registerSubsystem(this);
}
/**
* Gets the name of this Subsystem.
*
* @return Name
*/
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Subsystem.
*
* @param name name
*/
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Subsystem.
*
* @return Subsystem name
*/
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Subsystem.
*
* @param subsystem subsystem name
*/
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Associates a {@link Sendable} with this Subsystem. Also update the child's name.
*
* @param name name to give child
* @param child sendable
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
builder.addStringProperty(
".default",
() -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none",
null);
builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
builder.addStringProperty(
".command",
() -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none",
null);
}
}
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("PMD.AbstractClassWithoutAnyMethod")
public abstract class SubsystemBase extends Subsystem {}

View File

@@ -31,7 +31,7 @@ import java.util.function.Supplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class SwerveControllerCommand extends CommandBase {
public class SwerveControllerCommand extends Command {
private final Timer m_timer = new Timer();
private final Trajectory m_trajectory;
private final Supplier<Pose2d> m_pose;

View File

@@ -16,7 +16,7 @@ import java.util.function.Consumer;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class TrapezoidProfileCommand extends CommandBase {
public class TrapezoidProfileCommand extends Command {
private final TrapezoidProfile m_profile;
private final Consumer<State> m_output;

View File

@@ -14,7 +14,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
public abstract class TrapezoidProfileSubsystem extends Subsystem {
private final double m_period;
private final TrapezoidProfile.Constraints m_constraints;

View File

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.Timer;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class WaitCommand extends CommandBase {
public class WaitCommand extends Command {
protected Timer m_timer = new Timer();
private final double m_duration;

View File

@@ -15,7 +15,7 @@ import java.util.function.BooleanSupplier;
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class WaitUntilCommand extends CommandBase {
public class WaitUntilCommand extends Command {
private final BooleanSupplier m_condition;
/**

View File

@@ -14,7 +14,7 @@ import java.util.Set;
* added to any other composition or scheduled individually, and the composition requires all
* subsystems its components require.
*/
public abstract class WrapperCommand extends CommandBase {
public abstract class WrapperCommand extends Command {
protected final Command m_command;
/**