Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands] Revamp Interruptible (#4192)

Browse Source
This commit is contained in:
Starlight220
2022-08-30 07:53:47 +03:00
committed by GitHub
parent f2a8d38d2a
commit c3a93fb995
26 changed files with 369 additions and 592 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
View File

@@ -47,14 +47,15 @@ public interface Command {
/**
* Specifies the set of subsystems used by this command. Two commands cannot use the same
* subsystem at the same time. If the command is scheduled as interruptible and another command is
* scheduled that shares a requirement, the command will be interrupted. Else, the command will
* not be scheduled. If no subsystems are required, return an empty set.
* subsystem at the same time. If another command is scheduled that shares a requirement, {@link
* #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return
* an empty set.
*
* <p>Note: it is recommended that user implementations contain the requirements as a field, and
* return that field here, rather than allocating a new set every time this is called.
*
* @return the set of subsystems that are required
* @see InterruptionBehavior
*/
Set<Subsystem> getRequirements();
@@ -331,23 +332,30 @@ public interface Command {
}
/**
* Schedules this command.
* Decorates this command to have a different {@link InterruptionBehavior interruption behavior}.
*
* @param interruptible whether this command can be interrupted by another command that shares one
* of its requirements
* @param interruptBehavior the desired interrupt behavior
* @return the decorated command
*/
default void schedule(boolean interruptible) {
CommandScheduler.getInstance().schedule(interruptible, this);
default WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
return new WrapperCommand(this) {
@Override
public InterruptionBehavior getInterruptionBehavior() {
return interruptBehavior;
}
};
}
/** Schedules this command, defaulting to interruptible. */
/** Schedules this command. */
default void schedule() {
schedule(true);
CommandScheduler.getInstance().schedule(this);
}
/**
* Cancels this command. Will call the command's end() method with interrupted=true. Commands will
* be canceled even if they are not marked as interruptible.
* Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled
* regardless of {@link InterruptionBehavior interruption behavior}.
*
* @see CommandScheduler#cancel(Command...)
*/
default void cancel() {
CommandScheduler.getInstance().cancel(this);
@@ -373,6 +381,16 @@ public interface Command {
return getRequirements().contains(requirement);
}
/**
* How the command behaves when another command with a shared requirement is scheduled.
*
* @return a variant of {@link InterruptionBehavior}, defaulting to {@link
* InterruptionBehavior#kCancelSelf kCancelSelf}.
*/
default InterruptionBehavior getInterruptionBehavior() {
return InterruptionBehavior.kCancelSelf;
}
/**
* Whether the given command should run when the robot is disabled. Override to return true if the
* command should run when disabled.
@@ -391,4 +409,20 @@ public interface Command {
default String getName() {
return this.getClass().getSimpleName();
}
/**
* An enum describing the command's behavior when another command with a shared requirement is
* scheduled.
*/
enum InterruptionBehavior {
/**
* This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
* scheduled normally.
*
* <p>This is the default behavior.
*/
kCancelSelf,
/** This command continues, and the incoming command is not scheduled. */
kCancelIncoming
}
}

102
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -20,14 +20,14 @@ import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.Set;
import java.util.function.Consumer;
@@ -56,11 +56,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
return instance;
}
// A map from commands to their scheduling state. Also used as a set of the currently-running
// commands.
private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
// A set of the currently-running commands.
private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
// A map from required subsystems to their requiring commands. Also used as a set of the
// A map from required subsystems to their requiring commands. Also used as a set of the
// currently-required subsystems.
private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
@@ -83,7 +82,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Flag and queues for avoiding ConcurrentModificationException if commands are
// scheduled/canceled during run
private boolean m_inRunLoop;
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
private final Set<Command> m_toSchedule = new LinkedHashSet<>();
private final List<Command> m_toCancel = new ArrayList<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
@@ -103,7 +102,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
}
/**
* Changes the period of the loop overrun watchdog. This should be be kept in sync with the
* Changes the period of the loop overrun watchdog. This should be kept in sync with the
* TimedRobot period.
*
* @param period Period in seconds.
@@ -151,7 +150,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Adds a button binding to the scheduler, which will be polled to schedule commands.
*
* @param button The button to add
* @deprecated Use {@link Trigger}
* @deprecated Use {@link edu.wpi.first.wpilibj2.command.button.Trigger}
*/
@Deprecated(since = "2023")
public void addButton(Runnable button) {
@@ -172,12 +171,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Initializes a given command, adds its requirements to the list, and performs the init actions.
*
* @param command The command to initialize
* @param interruptible Whether the command is interruptible
* @param requirements The command requirements
*/
private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
CommandState scheduledCommand = new CommandState(interruptible);
m_scheduledCommands.put(command, scheduledCommand);
private void initCommand(Command command, Set<Subsystem> requirements) {
m_scheduledCommands.add(command);
for (Subsystem requirement : requirements) {
m_requirements.put(requirement, command);
}
@@ -195,17 +192,15 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* using those requirements have been scheduled as interruptible. If this is the case, they will
* be interrupted and the command will be scheduled.
*
* @param interruptible whether this command can be interrupted.
* @param command the command to schedule. If null, no-op.
*/
private void schedule(boolean interruptible, Command command) {
private void schedule(Command command) {
if (command == null) {
DriverStation.reportWarning("Tried to schedule a null command", true);
return;
}
if (m_inRunLoop) {
m_toSchedule.put(command, interruptible);
m_toSchedule.add(command);
return;
}
@@ -226,16 +221,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Schedule the command if the requirements are not currently in-use.
if (Collections.disjoint(m_requirements.keySet(), requirements)) {
initCommand(command, interruptible, requirements);
initCommand(command, requirements);
} else {
// Else check if the requirements that are in use have all have interruptible commands,
// and if so, interrupt those commands and schedule the new command.
for (Subsystem requirement : requirements) {
Command requiring = requiring(requirement);
if (requiring != null
&& !Optional.ofNullable(m_scheduledCommands.get(requiring))
.map(CommandState::isInterruptible)
.orElse(true)) {
&& requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
return;
}
}
@@ -245,33 +238,19 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
cancel(requiring);
}
}
initCommand(command, interruptible, requirements);
initCommand(command, requirements);
}
}
/**
* Schedules multiple commands for execution. Does nothing if the command is already scheduled. If
* a command's requirements are not available, it will only be started if all the commands
* currently using those requirements have been scheduled as interruptible. If this is the case,
* they will be interrupted and the command will be scheduled.
*
* @param interruptible whether the commands should be interruptible
* @param commands the commands to schedule. No-op if null.
*/
public void schedule(boolean interruptible, Command... commands) {
for (Command command : commands) {
schedule(interruptible, command);
}
}
/**
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
* if the command is already scheduled.
* Schedules multiple commands for execution. Does nothing for commands already scheduled.
*
* @param commands the commands to schedule. No-op on null.
*/
public void schedule(Command... commands) {
schedule(true, commands);
for (Command command : commands) {
schedule(command);
}
}
/**
@@ -312,8 +291,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = true;
// Run scheduled commands, remove finished commands.
for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
iterator.hasNext(); ) {
for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
@@ -346,8 +324,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = false;
// Schedule/cancel commands from queues populated during loop
for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
for (Command command : m_toSchedule) {
schedule(command);
}
for (Command command : m_toCancel) {
@@ -427,6 +405,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
throw new IllegalArgumentException("Default commands should not end!");
}
if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
DriverStation.reportWarning(
"Registering a non-interruptible default command!\n"
+ "This will likely prevent any other commands from requiring this subsystem.",
true);
// Warn, but allow -- there might be a use case for this.
}
m_subsystems.put(subsystem, defaultCommand);
}
@@ -446,7 +432,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* canceled command with {@code true}, indicating they were canceled (as opposed to finishing
* normally).
*
* <p>Commands will be canceled even if they are not scheduled as interruptible.
* <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}.
*
* @param commands the commands to cancel
*/
@@ -477,26 +463,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
/** Cancels all commands that are currently scheduled. */
public void cancelAll() {
for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
cancel(command);
}
}
/**
* Returns the time since a given command was scheduled. Note that this only works on commands
* that are directly scheduled by the scheduler; it will not work on commands inside of
* commandgroups, as the scheduler does not see them.
*
* @param command the command to query
* @return the time since the command was scheduled, in seconds
*/
public double timeSinceScheduled(Command command) {
CommandState commandState = m_scheduledCommands.get(command);
if (commandState != null) {
return commandState.timeSinceInitialized();
} else {
return -1;
}
// Copy to array to avoid concurrent modification.
cancel(m_scheduledCommands.toArray(new Command[0]));
}
/**
@@ -508,7 +476,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* @return whether the command is currently scheduled
*/
public boolean isScheduled(Command... commands) {
return m_scheduledCommands.keySet().containsAll(Set.of(commands));
return m_scheduledCommands.containsAll(Set.of(commands));
}
/**
@@ -583,7 +551,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
Map<Double, Command> ids = new LinkedHashMap<>();
for (Command command : m_scheduledCommands.keySet()) {
for (Command command : m_scheduledCommands) {
ids.put((double) command.hashCode(), command);
}

42
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
View File

@@ -1,42 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.Timer;
/**
* Class that holds scheduling state for a command. Used internally by the {@link CommandScheduler}.
*
* <p>This class is provided by the NewCommands VendorDep
*/
class CommandState {
// The time since this command was initialized.
private double m_startTime = -1;
// Whether or not it is interruptible.
private final boolean m_interruptible;
CommandState(boolean interruptible) {
m_interruptible = interruptible;
startTiming();
startRunning();
}
private void startTiming() {
m_startTime = Timer.getFPGATimestamp();
}
synchronized void startRunning() {
m_startTime = -1;
}
boolean isInterruptible() {
return m_interruptible;
}
double timeSinceInitialized() {
return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
}
}

5
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
View File

@@ -95,6 +95,11 @@ public abstract class WrapperCommand implements Command {
return m_command.runsWhenDisabled();
}
@Override
public InterruptionBehavior getInterruptionBehavior() {
return m_command.getInterruptionBehavior();
}
/**
* Gets the name of this Command.
*

72
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
View File

@@ -40,19 +40,6 @@ public class Button extends Trigger {
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command, boolean interruptible) {
whenActive(command, interruptible);
return this;
}
/**
* Starts the given command whenever the button is newly pressed. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command) {
@@ -75,23 +62,8 @@ public class Button extends Trigger {
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
* will be canceled when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whileHeld(final Command command, boolean interruptible) {
whileActiveContinuous(command, interruptible);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
* will be canceled when the button is released. The command is set to be interruptible.
* <p>{@link Command#schedule()} will be called repeatedly while the button is held, and will be
* canceled when the button is released.
*
* @param command the command to start
* @return this button, so calls can be chained
@@ -118,36 +90,10 @@ public class Button extends Trigger {
* but does not start it again if it ends or is otherwise interrupted.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command, boolean interruptible) {
whileActiveOnce(command, interruptible);
return this;
}
/**
* Starts the given command when the button is first pressed, and cancels it when it is released,
* but does not start it again if it ends or is otherwise interrupted. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command) {
whileActiveOnce(command, true);
return this;
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenReleased(final Command command, boolean interruptible) {
whenInactive(command, interruptible);
whileActiveOnce(command);
return this;
}
@@ -174,18 +120,6 @@ public class Button extends Trigger {
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button toggleWhenPressed(final Command command, boolean interruptible) {
toggleWhenActive(command, interruptible);
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on). The command is set to
* be interruptible.

102
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
View File

@@ -80,25 +80,13 @@ public class Trigger extends BooleanEvent {
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenActive");
this.rising().ifHigh(() -> command.schedule(interruptible));
return this;
}
/**
* Starts the given command whenever the trigger just becomes active. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command) {
return whenActive(command, true);
requireNonNullParam(command, "command", "whenActive");
this.rising().ifHigh(command::schedule);
return this;
}
/**
@@ -115,33 +103,19 @@ public class Trigger extends BooleanEvent {
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whileActiveContinuous");
this.ifHigh(() -> command.schedule(interruptible));
this.falling().ifHigh(command::cancel);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive.
* <p>{@link Command#schedule()} will be called repeatedly while the trigger is active, and will
* be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command) {
return whileActiveContinuous(command, true);
requireNonNullParam(command, "command", "whileActiveContinuous");
this.ifHigh(command::schedule);
this.falling().ifHigh(command::cancel);
return this;
}
/**
@@ -160,52 +134,29 @@ public class Trigger extends BooleanEvent {
* inactive, but does not re-start it in-between.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command, boolean interruptible) {
public Trigger whileActiveOnce(final Command command) {
requireNonNullParam(command, "command", "whileActiveOnce");
this.rising().ifHigh(() -> command.schedule(interruptible));
this.rising().ifHigh(command::schedule);
this.falling().ifHigh(command::cancel);
return this;
}
/**
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
* inactive, but does not re-start it in-between. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command) {
return whileActiveOnce(command, true);
}
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenInactive");
this.falling().ifHigh(() -> command.schedule(interruptible));
return this;
}
/**
* Starts the command when the trigger becomes inactive. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command) {
return whenInactive(command, true);
requireNonNullParam(command, "command", "whenInactive");
this.falling().ifHigh(command::schedule);
return this;
}
/**
@@ -223,10 +174,9 @@ public class Trigger extends BooleanEvent {
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command, boolean interruptible) {
public Trigger toggleWhenActive(final Command command) {
requireNonNullParam(command, "command", "toggleWhenActive");
this.rising()
@@ -235,23 +185,13 @@ public class Trigger extends BooleanEvent {
if (command.isScheduled()) {
command.cancel();
} else {
command.schedule(interruptible);
command.schedule();
}
});
return this;
}
/**
* Toggles a command when the trigger becomes active. The command is set to be interruptible.
*
* @param command the command to toggle
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command) {
return toggleWhenActive(command, true);
}
/**
* Cancels a command when the trigger becomes active.
*

25
wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
View File

@@ -67,6 +67,27 @@ std::unique_ptr<Command> Command::IgnoringDisable(bool doesRunWhenDisabled) && {
std::move(*this).TransferOwnership(), doesRunWhenDisabled);
}
std::unique_ptr<Command> Command::WithInterruptBehavior(
InterruptionBehavior interruptBehavior) && {
class InterruptBehaviorCommand
: public CommandHelper<WrapperCommand, InterruptBehaviorCommand> {
public:
InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
InterruptionBehavior interruptBehavior)
: CommandHelper(std::move(command)),
m_interruptBehavior(interruptBehavior) {}
InterruptionBehavior GetInterruptionBehavior() const override {
return m_interruptBehavior;
}
private:
InterruptionBehavior m_interruptBehavior;
};
return std::make_unique<InterruptBehaviorCommand>(
std::move(*this).TransferOwnership(), interruptBehavior);
}
ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
std::vector<std::unique_ptr<Command>> temp;
temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
@@ -130,8 +151,8 @@ ConditionalCommand Command::Unless(std::function<bool()> condition) && {
std::move(condition));
}
void Command::Schedule(bool interruptible) {
CommandScheduler::GetInstance().Schedule(interruptible, this);
void Command::Schedule() {
CommandScheduler::GetInstance().Schedule(this);
}
void Command::Cancel() {

83
wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
View File

@@ -19,16 +19,14 @@
#include <wpi/sendable/SendableRegistry.h>
#include "frc2/command/CommandGroupBase.h"
#include "frc2/command/CommandState.h"
#include "frc2/command/Subsystem.h"
using namespace frc2;
class CommandScheduler::Impl {
public:
// A map from commands to their scheduling state. Also used as a set of the
// currently-running commands.
wpi::DenseMap<Command*, CommandState> scheduledCommands;
// A set of the currently-running commands.
wpi::SmallSet<Command*, 12> scheduledCommands;
// A map from required subsystems to their requiring commands. Also used as a
// set of the currently-required subsystems.
@@ -56,7 +54,7 @@ class CommandScheduler::Impl {
// scheduled/canceled during run
bool inRunLoop = false;
wpi::DenseMap<Command*, bool> toSchedule;
wpi::SmallVector<Command*, 4> toSchedule;
wpi::SmallVector<Command*, 4> toCancel;
};
@@ -112,9 +110,9 @@ void CommandScheduler::ClearButtons() {
m_impl->activeButtonLoop->Clear();
}
void CommandScheduler::Schedule(bool interruptible, Command* command) {
void CommandScheduler::Schedule(Command* command) {
if (m_impl->inRunLoop) {
m_impl->toSchedule.try_emplace(command, interruptible);
m_impl->toSchedule.emplace_back(command);
return;
}
@@ -126,7 +124,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
}
if (m_impl->disabled ||
(frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
ContainsKey(m_impl->scheduledCommands, command)) {
m_impl->scheduledCommands.contains(command)) {
return;
}
@@ -139,8 +137,8 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
for (auto&& i1 : m_impl->requirements) {
if (requirements.find(i1.first) != requirements.end()) {
isDisjoint = false;
allInterruptible &=
m_impl->scheduledCommands[i1.second].IsInterruptible();
allInterruptible &= (i1.second->GetInterruptionBehavior() ==
Command::InterruptionBehavior::kCancelSelf);
intersection.emplace_back(i1.second);
}
}
@@ -151,7 +149,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
Cancel(cmdToCancel);
}
}
m_impl->scheduledCommands[command] = CommandState{interruptible};
m_impl->scheduledCommands.insert(command);
for (auto&& requirement : requirements) {
m_impl->requirements[requirement] = command;
}
@@ -163,33 +161,15 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
}
}
void CommandScheduler::Schedule(Command* command) {
Schedule(true, command);
}
void CommandScheduler::Schedule(bool interruptible,
wpi::span<Command* const> commands) {
for (auto command : commands) {
Schedule(interruptible, command);
}
}
void CommandScheduler::Schedule(bool interruptible,
std::initializer_list<Command*> commands) {
for (auto command : commands) {
Schedule(interruptible, command);
}
}
void CommandScheduler::Schedule(wpi::span<Command* const> commands) {
for (auto command : commands) {
Schedule(true, command);
Schedule(command);
}
}
void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
for (auto command : commands) {
Schedule(true, command);
Schedule(command);
}
}
@@ -218,10 +198,7 @@ void CommandScheduler::Run() {
m_impl->inRunLoop = true;
// Run scheduled commands, remove finished commands.
for (auto iterator = m_impl->scheduledCommands.begin();
iterator != m_impl->scheduledCommands.end(); iterator++) {
Command* command = iterator->getFirst();
for (Command* command : m_impl->scheduledCommands) {
if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
Cancel(command);
continue;
@@ -243,14 +220,14 @@ void CommandScheduler::Run() {
m_impl->requirements.erase(requirement);
}
m_impl->scheduledCommands.erase(iterator);
m_impl->scheduledCommands.erase(command);
m_watchdog.AddEpoch(command->GetName() + ".End(false)");
}
}
m_impl->inRunLoop = false;
for (auto&& commandInterruptible : m_impl->toSchedule) {
Schedule(commandInterruptible.second, commandInterruptible.first);
for (Command* command : m_impl->toSchedule) {
Schedule(command);
}
for (auto&& command : m_impl->toCancel) {
@@ -336,7 +313,7 @@ void CommandScheduler::Cancel(Command* command) {
if (find == m_impl->scheduledCommands.end()) {
return;
}
m_impl->scheduledCommands.erase(find);
m_impl->scheduledCommands.erase(*find);
for (auto&& requirement : m_impl->requirements) {
if (requirement.second == command) {
m_impl->requirements.erase(requirement.first);
@@ -364,20 +341,11 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
void CommandScheduler::CancelAll() {
wpi::SmallVector<Command*, 16> commands;
for (auto&& command : m_impl->scheduledCommands) {
commands.emplace_back(command.first);
commands.emplace_back(command);
}
Cancel(commands);
}
units::second_t CommandScheduler::TimeSinceScheduled(
const Command* command) const {
auto find = m_impl->scheduledCommands.find(command);
if (find != m_impl->scheduledCommands.end()) {
return find->second.TimeSinceInitialized();
} else {
return -1_s;
}
}
bool CommandScheduler::IsScheduled(
wpi::span<const Command* const> commands) const {
for (auto command : commands) {
@@ -399,8 +367,7 @@ bool CommandScheduler::IsScheduled(
}
bool CommandScheduler::IsScheduled(const Command* command) const {
return m_impl->scheduledCommands.find(command) !=
m_impl->scheduledCommands.end();
return m_impl->scheduledCommands.contains(command);
}
Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
@@ -458,9 +425,9 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
wpi::SmallVector<std::string, 8> names;
wpi::SmallVector<double, 8> ids;
for (auto&& command : m_impl->scheduledCommands) {
names.emplace_back(command.first->GetName());
uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
for (Command* command : m_impl->scheduledCommands) {
names.emplace_back(command->GetName());
uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command);
ids.emplace_back(static_cast<double>(ptrTmp));
}
nt::NetworkTableEntry(namesEntry).SetStringArray(names);
@@ -470,5 +437,13 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
std::unique_ptr<Command> command) {
if (command->GetInterruptionBehavior() ==
Command::InterruptionBehavior::kCancelIncoming) {
std::puts(
"Registering a non-interruptible default command!\n"
"This will likely prevent any other commands from "
"requiring this subsystem.");
// Warn, but allow -- there might be a use case for this.
}
m_impl->subsystems[subsystem] = std::move(command);
}

27
wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
View File

@@ -1,27 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc2/command/CommandState.h"
#include <frc/Timer.h>
using namespace frc2;
CommandState::CommandState(bool interruptible)
: m_interruptible{interruptible} {
StartTiming();
StartRunning();
}
void CommandState::StartTiming() {
m_startTime = frc::Timer::GetFPGATimestamp();
}
void CommandState::StartRunning() {
m_startTime = -1_s;
}
units::second_t CommandState::TimeSinceInitialized() const {
return m_startTime != -1_s ? frc::Timer::GetFPGATimestamp() - m_startTime
: -1_s;
}

10
wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
View File

@@ -4,6 +4,8 @@
#include "frc2/command/WrapperCommand.h"
#include "frc2/command/Command.h"
using namespace frc2;
WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
@@ -33,3 +35,11 @@ void WrapperCommand::End(bool interrupted) {
bool WrapperCommand::RunsWhenDisabled() const {
return m_command->RunsWhenDisabled();
}
Command::InterruptionBehavior WrapperCommand::GetInterruptionBehavior() const {
return m_command->GetInterruptionBehavior();
}
wpi::SmallSet<Subsystem*, 4> WrapperCommand::GetRequirements() const {
return m_command->GetRequirements();
}

20
wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
View File

@@ -8,8 +8,8 @@ using namespace frc2;
Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
Button Button::WhenPressed(Command* command, bool interruptible) {
WhenActive(command, interruptible);
Button Button::WhenPressed(Command* command) {
WhenActive(command);
return *this;
}
@@ -25,8 +25,8 @@ Button Button::WhenPressed(std::function<void()> toRun,
return *this;
}
Button Button::WhileHeld(Command* command, bool interruptible) {
WhileActiveContinous(command, interruptible);
Button Button::WhileHeld(Command* command) {
WhileActiveContinous(command);
return *this;
}
@@ -42,13 +42,13 @@ Button Button::WhileHeld(std::function<void()> toRun,
return *this;
}
Button Button::WhenHeld(Command* command, bool interruptible) {
WhileActiveOnce(command, interruptible);
Button Button::WhenHeld(Command* command) {
WhileActiveOnce(command);
return *this;
}
Button Button::WhenReleased(Command* command, bool interruptible) {
WhenInactive(command, interruptible);
Button Button::WhenReleased(Command* command) {
WhenInactive(command);
return *this;
}
@@ -64,8 +64,8 @@ Button Button::WhenReleased(std::function<void()> toRun,
return *this;
}
Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
ToggleWhenActive(command, interruptible);
Button Button::ToggleWhenPressed(Command* command) {
ToggleWhenActive(command);
return *this;
}

25
wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
View File

@@ -13,9 +13,8 @@ using namespace frc2;
Trigger::Trigger(const Trigger& other) = default;
Trigger Trigger::WhenActive(Command* command, bool interruptible) {
this->Rising().IfHigh(
[command, interruptible] { command->Schedule(interruptible); });
Trigger Trigger::WhenActive(Command* command) {
this->Rising().IfHigh([command] { command->Schedule(); });
return *this;
}
@@ -30,8 +29,8 @@ Trigger Trigger::WhenActive(std::function<void()> toRun,
return WhenActive(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
this->IfHigh([command, interruptible] { command->Schedule(interruptible); });
Trigger Trigger::WhileActiveContinous(Command* command) {
this->IfHigh([command] { command->Schedule(); });
this->Falling().IfHigh([command] { command->Cancel(); });
return *this;
}
@@ -48,16 +47,14 @@ Trigger Trigger::WhileActiveContinous(
return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
this->Rising().IfHigh(
[command, interruptible] { command->Schedule(interruptible); });
Trigger Trigger::WhileActiveOnce(Command* command) {
this->Rising().IfHigh([command] { command->Schedule(); });
this->Falling().IfHigh([command] { command->Cancel(); });
return *this;
}
Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
this->Falling().IfHigh(
[command, interruptible] { command->Schedule(interruptible); });
Trigger Trigger::WhenInactive(Command* command) {
this->Falling().IfHigh([command] { command->Schedule(); });
return *this;
}
@@ -72,12 +69,12 @@ Trigger Trigger::WhenInactive(std::function<void()> toRun,
return WhenInactive(InstantCommand(std::move(toRun), requirements));
}
Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
this->Rising().IfHigh([command, interruptible] {
Trigger Trigger::ToggleWhenActive(Command* command) {
this->Rising().IfHigh([command] {
if (command->IsScheduled()) {
command->Cancel();
} else {
command->Schedule(interruptible);
command->Schedule();
}
});
return *this;

53
wpilibNewCommands/src/main/native/include/frc2/command/Command.h
View File

@@ -91,20 +91,36 @@ class Command {
virtual bool IsFinished() { return false; }
/**
* Specifies the set of subsystems used by this command. Two commands cannot
* use the same subsystem at the same time. If the command is scheduled as
* interruptible and another command is scheduled that shares a requirement,
* the command will be interrupted. Else, the command will not be scheduled.
* If no subsystems are required, return an empty set.
* Specifies the set of subsystems used by this command. Two commands cannot
* use the same subsystem at the same time. If another command is scheduled
* that shares a requirement, GetInterruptionBehavior() will be checked and
* followed. If no subsystems are required, return an empty set.
*
* <p>Note: it is recommended that user implementations contain the
* requirements as a field, and return that field here, rather than allocating
* a new set every time this is called.
*
* @return the set of subsystems that are required
* @see InterruptionBehavior
*/
virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
/**
* An enum describing the command's behavior when another command with a
* shared requirement is scheduled.
*/
enum class InterruptionBehavior {
/**
* This command ends, End(true) is called, and the incoming command is
* scheduled normally.
*
* <p>This is the default behavior.
*/
kCancelSelf,
/** This command continues, and the incoming command is not scheduled. */
kCancelIncoming
};
/**
* Decorates this command with a timeout. If the specified timeout is
* exceeded before the command finishes normally, the command will be
@@ -238,21 +254,22 @@ class Command {
virtual std::unique_ptr<Command> IgnoringDisable(bool doesRunWhenDisabled) &&;
/**
* Schedules this command.
* Decorates this command to run or stop when disabled.
*
* @param interruptible whether this command can be interrupted by another
* command that shares one of its requirements
* @param interruptBehavior true to run when disabled.
* @return the decorated command
*/
void Schedule(bool interruptible);
virtual std::unique_ptr<Command> WithInterruptBehavior(
InterruptionBehavior interruptBehavior) &&;
/**
* Schedules this command, defaulting to interruptible.
* Schedules this command.
*/
void Schedule() { Schedule(true); }
void Schedule();
/**
* Cancels this command. Will call the command's interrupted() method.
* Commands will be canceled even if they are not marked as interruptible.
* Cancels this command. Will call End(true). Commands will be canceled
* regardless of interruption behavior.
*/
void Cancel();
@@ -296,6 +313,16 @@ class Command {
*/
virtual bool RunsWhenDisabled() const { return false; }
/**
* How the command behaves when another command with a shared requirement is
* scheduled.
*
* @return a variant of InterruptionBehavior, defaulting to kCancelSelf.
*/
virtual InterruptionBehavior GetInterruptionBehavior() const {
return InterruptionBehavior::kCancelSelf;
}
virtual std::string GetName() const;
protected:

56
wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
View File

@@ -83,60 +83,27 @@ class CommandScheduler final : public nt::NTSendable,
void ClearButtons();
/**
* Schedules a command for execution. Does nothing if the command is already
* Schedules a command for execution. Does nothing if the command is already
* scheduled. If a command's requirements are not available, it will only be
* started if all the commands currently using those requirements have been
* scheduled as interruptible. If this is the case, they will be interrupted
* scheduled as interruptible. If this is the case, they will be interrupted
* and the command will be scheduled.
*
* @param interruptible whether this command can be interrupted
* @param command the command to schedule
*/
void Schedule(bool interruptible, Command* command);
/**
* Schedules a command for execution, with interruptible defaulted to true.
* Does nothing if the command is already scheduled.
*
* @param command the command to schedule
*/
void Schedule(Command* command);
/**
* Schedules multiple commands for execution. Does nothing if the command is
* already scheduled. If a command's requirements are not available, it will
* only be started if all the commands currently using those requirements have
* been scheduled as interruptible. If this is the case, they will be
* interrupted and the command will be scheduled.
*
* @param interruptible whether the commands should be interruptible
* @param commands the commands to schedule
*/
void Schedule(bool interruptible, wpi::span<Command* const> commands);
/**
* Schedules multiple commands for execution. Does nothing if the command is
* already scheduled. If a command's requirements are not available, it will
* only be started if all the commands currently using those requirements have
* been scheduled as interruptible. If this is the case, they will be
* interrupted and the command will be scheduled.
*
* @param interruptible whether the commands should be interruptible
* @param commands the commands to schedule
*/
void Schedule(bool interruptible, std::initializer_list<Command*> commands);
/**
* Schedules multiple commands for execution, with interruptible defaulted to
* true. Does nothing if the command is already scheduled.
* Schedules multiple commands for execution. Does nothing for commands
* already scheduled.
*
* @param commands the commands to schedule
*/
void Schedule(wpi::span<Command* const> commands);
/**
* Schedules multiple commands for execution, with interruptible defaulted to
* true. Does nothing if the command is already scheduled.
* Schedules multiple commands for execution. Does nothing for commands
* already scheduled.
*
* @param commands the commands to schedule
*/
@@ -262,17 +229,6 @@ class CommandScheduler final : public nt::NTSendable,
*/
void CancelAll();
/**
* Returns the time since a given command was scheduled. Note that this only
* works on commands that are directly scheduled by the scheduler; it will not
* work on commands inside of commandgroups, as the scheduler does not see
* them.
*
* @param command the command to query
* @return the time since the command was scheduled
*/
units::second_t TimeSinceScheduled(const Command* command) const;
/**
* Whether the given commands are running. Note that this only works on
* commands that are directly scheduled by the scheduler; it will not work on

34
wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
View File

@@ -1,34 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <units/time.h>
namespace frc2 {
/**
* Class that holds scheduling state for a command. Used internally by the
* CommandScheduler
*
* This class is provided by the NewCommands VendorDep
*/
class CommandState final {
public:
CommandState() = default;
explicit CommandState(bool interruptible);
bool IsInterruptible() const { return m_interruptible; }
// The time since this command was initialized.
units::second_t TimeSinceInitialized() const;
private:
units::second_t m_startTime = -1_s;
bool m_interruptible;
void StartTiming();
void StartRunning();
};
} // namespace frc2

4
wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
View File

@@ -64,6 +64,10 @@ class WrapperCommand : public CommandHelper<CommandBase, WrapperCommand> {
bool RunsWhenDisabled() const override;
InterruptionBehavior GetInterruptionBehavior() const override;
wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
protected:
std::unique_ptr<Command> m_command;
};

40
wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
View File

@@ -43,10 +43,9 @@ class Button : public Trigger {
* of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Button WhenPressed(Command* command, bool interruptible = true);
Button WhenPressed(Command* command);
/**
* Binds a command to start when the button is pressed. Transfers
@@ -55,13 +54,12 @@ class Button : public Trigger {
* *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Button WhenPressed(T&& command, bool interruptible = true) {
WhenActive(std::forward<T>(command), interruptible);
Button WhenPressed(T&& command) {
WhenActive(std::forward<T>(command));
return *this;
}
@@ -89,10 +87,9 @@ class Button : public Trigger {
* users are responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
Button WhileHeld(Command* command, bool interruptible = true);
Button WhileHeld(Command* command);
/**
* Binds a command to be started repeatedly while the button is pressed, and
@@ -101,13 +98,12 @@ class Button : public Trigger {
* will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Button WhileHeld(T&& command, bool interruptible = true) {
WhileActiveContinous(std::forward<T>(command), interruptible);
Button WhileHeld(T&& command) {
WhileActiveContinous(std::forward<T>(command));
return *this;
}
@@ -135,10 +131,9 @@ class Button : public Trigger {
* responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
Button WhenHeld(Command* command, bool interruptible = true);
Button WhenHeld(Command* command);
/**
* Binds a command to be started when the button is pressed, and canceled
@@ -147,13 +142,12 @@ class Button : public Trigger {
* *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Button WhenHeld(T&& command, bool interruptible = true) {
WhileActiveOnce(std::forward<T>(command), interruptible);
Button WhenHeld(T&& command) {
WhileActiveOnce(std::forward<T>(command));
return *this;
}
@@ -163,10 +157,9 @@ class Button : public Trigger {
* of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
Button WhenReleased(Command* command, bool interruptible = true);
Button WhenReleased(Command* command);
/**
* Binds a command to start when the button is pressed. Transfers
@@ -175,13 +168,12 @@ class Button : public Trigger {
* *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Button WhenReleased(T&& command, bool interruptible = true) {
WhenInactive(std::forward<T>(command), interruptible);
Button WhenReleased(T&& command) {
WhenInactive(std::forward<T>(command));
return *this;
}
@@ -209,10 +201,9 @@ class Button : public Trigger {
* responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
Button ToggleWhenPressed(Command* command, bool interruptible = true);
Button ToggleWhenPressed(Command* command);
/**
* Binds a command to start when the button is pressed, and be canceled when
@@ -221,13 +212,12 @@ class Button : public Trigger {
* *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Button ToggleWhenPressed(T&& command, bool interruptible = true) {
ToggleWhenActive(std::forward<T>(command), interruptible);
Button ToggleWhenPressed(T&& command) {
ToggleWhenActive(std::forward<T>(command));
return *this;
}

54
wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
View File

@@ -60,33 +60,30 @@ class Trigger : public frc::BooleanEvent {
Trigger(const Trigger& other);
/**
* Binds a command to start when the trigger becomes active. Takes a
* Binds a command to start when the trigger becomes active. Takes a
* raw pointer, and so is non-owning; users are responsible for the lifespan
* of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Trigger WhenActive(Command* command, bool interruptible = true);
Trigger WhenActive(Command* command);
/**
* Binds a command to start when the trigger becomes active. Transfers
* Binds a command to start when the trigger becomes active. Transfers
* command ownership to the button scheduler, so the user does not have to
* worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
* *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Trigger WhenActive(T&& command, bool interruptible = true) {
Trigger WhenActive(T&& command) {
this->Rising().IfHigh(
[command = std::make_unique<std::remove_reference_t<T>>(
std::forward<T>(command)),
interruptible] { command->Schedule(interruptible); });
std::forward<T>(command))] { command->Schedule(); });
return *this;
}
@@ -115,10 +112,9 @@ class Trigger : public frc::BooleanEvent {
* non-owning; users are responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Trigger WhileActiveContinous(Command* command, bool interruptible = true);
Trigger WhileActiveContinous(Command* command);
/**
* Binds a command to be started repeatedly while the trigger is active, and
@@ -127,15 +123,14 @@ class Trigger : public frc::BooleanEvent {
* rvalue refs will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
Trigger WhileActiveContinous(T&& command) {
std::shared_ptr<T> ptr =
std::make_shared<std::remove_reference_t<T>>(std::forward<T>(command));
this->IfHigh([ptr, interruptible] { ptr->Schedule(interruptible); });
this->IfHigh([ptr] { ptr->Schedule(); });
this->Falling().IfHigh([ptr] { ptr->Cancel(); });
return *this;
@@ -165,29 +160,26 @@ class Trigger : public frc::BooleanEvent {
* non-owning; users are responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Trigger WhileActiveOnce(Command* command, bool interruptible = true);
Trigger WhileActiveOnce(Command* command);
/**
* Binds a command to be started when the trigger becomes active, and
* canceled when it becomes inactive. Transfers command ownership to the
* canceled when it becomes inactive. Transfers command ownership to the
* button scheduler, so the user does not have to worry about lifespan -
* rvalue refs will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
Trigger WhileActiveOnce(T&& command) {
std::shared_ptr<T> ptr =
std::make_shared<std::remove_reference_t<T>>(std::forward<T>(command));
this->Rising().IfHigh(
[ptr, interruptible] { ptr->Schedule(interruptible); });
this->Rising().IfHigh([ptr] { ptr->Schedule(); });
this->Falling().IfHigh([ptr] { ptr->Cancel(); });
return *this;
@@ -199,10 +191,9 @@ class Trigger : public frc::BooleanEvent {
* of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Trigger WhenInactive(Command* command, bool interruptible = true);
Trigger WhenInactive(Command* command);
/**
* Binds a command to start when the trigger becomes inactive. Transfers
@@ -211,16 +202,14 @@ class Trigger : public frc::BooleanEvent {
* *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Trigger WhenInactive(T&& command, bool interruptible = true) {
Trigger WhenInactive(T&& command) {
this->Falling().IfHigh(
[command = std::make_unique<std::remove_reference_t<T>>(
std::forward<T>(command)),
interruptible] { command->Schedule(interruptible); });
std::forward<T>(command))] { command->Schedule(); });
return *this;
}
@@ -245,14 +234,13 @@ class Trigger : public frc::BooleanEvent {
/**
* Binds a command to start when the trigger becomes active, and be canceled
* when it again becomes active. Takes a raw pointer, and so is non-owning;
* when it again becomes active. Takes a raw pointer, and so is non-owning;
* users are responsible for the lifespan of the command.
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
Trigger ToggleWhenActive(Command* command, bool interruptible = true);
Trigger ToggleWhenActive(Command* command);
/**
* Binds a command to start when the trigger becomes active, and be canceled
@@ -261,18 +249,16 @@ class Trigger : public frc::BooleanEvent {
* will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
* @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template <class T, typename = std::enable_if_t<std::is_base_of_v<
Command, std::remove_reference_t<T>>>>
Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
Trigger ToggleWhenActive(T&& command) {
this->Rising().IfHigh(
[command = std::make_unique<std::remove_reference_t<T>>(
std::forward<T>(command)),
interruptible] {
std::forward<T>(command))] {
if (!command->IsScheduled()) {
command->Schedule(interruptible);
command->Schedule();
} else {
command->Cancel();
}

7
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
View File

@@ -44,12 +44,13 @@ class CommandRequirementsTest extends CommandTestBase {
try (CommandScheduler scheduler = new CommandScheduler()) {
Subsystem requirement = new SubsystemBase() {};
MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
Command notInterrupted = interruptedHolder.getMock();
Command notInterrupted =
new RunCommand(() -> {}, requirement)
.withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming);
MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
Command interrupter = interrupterHolder.getMock();
scheduler.schedule(false, notInterrupted);
scheduler.schedule(notInterrupted);
scheduler.schedule(interrupter);
assertTrue(scheduler.isScheduled(notInterrupted));

2
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
View File

@@ -66,7 +66,7 @@ class CommandScheduleTest extends CommandTestBase {
MockCommandHolder command3Holder = new MockCommandHolder(true);
Command command3 = command3Holder.getMock();
scheduler.schedule(true, command1, command2, command3);
scheduler.schedule(command1, command2, command3);
assertTrue(scheduler.isScheduled(command1, command2, command3));
scheduler.run();
assertTrue(scheduler.isScheduled(command1, command2, command3));

35
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
View File

@@ -9,19 +9,20 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.ValueSource;
import org.junit.jupiter.params.provider.EnumSource;
class SchedulingRecursionTest extends CommandTestBase {
/**
* <a href="https://github.com/wpilibsuite/allwpilib/issues/4259">wpilibsuite/allwpilib#4259</a>.
*/
@ValueSource(booleans = {true, false})
@EnumSource(InterruptionBehavior.class)
@ParameterizedTest
void cancelFromInitialize(boolean interruptible) {
void cancelFromInitialize(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicBoolean hasOtherRun = new AtomicBoolean();
Subsystem requirement = new SubsystemBase() {};
@@ -35,14 +36,18 @@ class SchedulingRecursionTest extends CommandTestBase {
public void initialize() {
scheduler.cancel(this);
}
@Override
public InterruptionBehavior getInterruptionBehavior() {
return interruptionBehavior;
}
};
Command other = new RunCommand(() -> hasOtherRun.set(true), requirement);
assertDoesNotThrow(
() -> {
scheduler.schedule(interruptible, selfCancels);
scheduler.schedule(selfCancels);
scheduler.run();
// interruptibility of new arrival isn't checked
scheduler.schedule(other);
});
assertFalse(scheduler.isScheduled(selfCancels));
@@ -52,9 +57,9 @@ class SchedulingRecursionTest extends CommandTestBase {
}
}
@ValueSource(booleans = {true, false})
@EnumSource(InterruptionBehavior.class)
@ParameterizedTest
void defaultCommand(boolean interruptible) {
void defaultCommandGetsRescheduledAfterSelfCanceling(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicBoolean hasOtherRun = new AtomicBoolean();
Subsystem requirement = new SubsystemBase() {};
@@ -68,13 +73,18 @@ class SchedulingRecursionTest extends CommandTestBase {
public void initialize() {
scheduler.cancel(this);
}
@Override
public InterruptionBehavior getInterruptionBehavior() {
return interruptionBehavior;
}
};
Command other = new RunCommand(() -> hasOtherRun.set(true), requirement);
scheduler.setDefaultCommand(requirement, other);
assertDoesNotThrow(
() -> {
scheduler.schedule(interruptible, selfCancels);
scheduler.schedule(selfCancels);
scheduler.run();
});
scheduler.run();
@@ -161,12 +171,13 @@ class SchedulingRecursionTest extends CommandTestBase {
}
@ParameterizedTest
@ValueSource(booleans = {true, false})
void scheduleInitializeFromDefaultCommand(boolean interruptible) {
@EnumSource(InterruptionBehavior.class)
void scheduleInitializeFromDefaultCommand(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicInteger counter = new AtomicInteger();
Subsystem requirement = new SubsystemBase() {};
Command other = new InstantCommand(() -> {}, requirement);
Command other =
new InstantCommand(() -> {}, requirement).withInterruptBehavior(interruptionBehavior);
Command defaultCommand =
new CommandBase() {
{
@@ -176,7 +187,7 @@ class SchedulingRecursionTest extends CommandTestBase {
@Override
public void initialize() {
counter.incrementAndGet();
scheduler.schedule(interruptible, other);
scheduler.schedule(other);
}
};

24
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
View File

@@ -31,11 +31,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whenPressed(command1);
scheduler.run();
verify(command1, never()).schedule(true);
verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
verify(command1).schedule(true);
verify(command1).schedule();
}
@Test
@@ -48,11 +48,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(true);
button.whenReleased(command1);
scheduler.run();
verify(command1, never()).schedule(true);
verify(command1, never()).schedule();
button.setPressed(false);
scheduler.run();
scheduler.run();
verify(command1).schedule(true);
verify(command1).schedule();
}
@Test
@@ -65,11 +65,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whileHeld(command1);
scheduler.run();
verify(command1, never()).schedule(true);
verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
verify(command1, times(2)).schedule(true);
verify(command1, times(2)).schedule();
button.setPressed(false);
scheduler.run();
verify(command1).cancel();
@@ -85,11 +85,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whenHeld(command1);
scheduler.run();
verify(command1, never()).schedule(true);
verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
verify(command1).schedule(true);
verify(command1).schedule();
button.setPressed(false);
scheduler.run();
verify(command1).cancel();
@@ -105,12 +105,12 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.toggleWhenPressed(command1);
scheduler.run();
verify(command1, never()).schedule(true);
verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
when(command1.isScheduled()).thenReturn(true);
scheduler.run();
verify(command1).schedule(true);
verify(command1).schedule();
button.setPressed(false);
scheduler.run();
verify(command1, never()).cancel();
@@ -200,13 +200,13 @@ class ButtonTest extends CommandTestBase {
button.setPressed(true);
scheduler.run();
verify(command, never()).schedule(true);
verify(command, never()).schedule();
SimHooks.stepTiming(0.3);
button.setPressed(true);
scheduler.run();
verify(command).schedule(true);
verify(command).schedule();
}
@Test

4
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
View File

@@ -37,10 +37,10 @@ class NetworkButtonTest extends CommandTestBase {
entry.setBoolean(false);
button.whenPressed(command);
scheduler.run();
verify(command, never()).schedule(true);
verify(command, never()).schedule();
entry.setBoolean(true);
scheduler.run();
scheduler.run();
verify(command).schedule(true);
verify(command).schedule();
}
}

28
wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
View File

@@ -7,6 +7,7 @@
#include "CommandTestBase.h"
#include "frc2/command/CommandScheduler.h"
#include "frc2/command/ConditionalCommand.h"
#include "frc2/command/FunctionalCommand.h"
#include "frc2/command/InstantCommand.h"
#include "frc2/command/ParallelCommandGroup.h"
#include "frc2/command/ParallelDeadlineGroup.h"
@@ -49,26 +50,35 @@ TEST_F(CommandRequirementsTest, RequirementUninterruptible) {
TestSubsystem requirement;
MockCommand command1({&requirement});
MockCommand command2({&requirement});
int initCounter = 0;
int exeCounter = 0;
int endCounter = 0;
EXPECT_CALL(command1, Initialize());
EXPECT_CALL(command1, Execute()).Times(2);
EXPECT_CALL(command1, End(true)).Times(0);
EXPECT_CALL(command1, End(false)).Times(0);
std::unique_ptr<Command> command1 =
FunctionalCommand([&initCounter] { initCounter++; },
[&exeCounter] { exeCounter++; },
[&endCounter](bool interruptible) { endCounter++; },
[] { return false; }, {&requirement})
.WithInterruptBehavior(
Command::InterruptionBehavior::kCancelIncoming);
MockCommand command2({&requirement});
EXPECT_CALL(command2, Initialize()).Times(0);
EXPECT_CALL(command2, Execute()).Times(0);
EXPECT_CALL(command2, End(true)).Times(0);
EXPECT_CALL(command2, End(false)).Times(0);
scheduler.Schedule(false, &command1);
scheduler.Schedule(command1.get());
EXPECT_EQ(1, initCounter);
scheduler.Run();
EXPECT_TRUE(scheduler.IsScheduled(&command1));
EXPECT_EQ(1, exeCounter);
EXPECT_TRUE(scheduler.IsScheduled(command1.get()));
scheduler.Schedule(&command2);
EXPECT_TRUE(scheduler.IsScheduled(&command1));
EXPECT_TRUE(scheduler.IsScheduled(command1.get()));
EXPECT_FALSE(scheduler.IsScheduled(&command2));
scheduler.Run();
EXPECT_EQ(2, exeCounter);
EXPECT_EQ(0, endCounter);
}
TEST_F(CommandRequirementsTest, DefaultCommandRequirementError) {

13
wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
View File

@@ -10,6 +10,7 @@
#include <frc/simulation/DriverStationSim.h>
#include "frc2/command/CommandGroupBase.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/CommandScheduler.h"
#include "frc2/command/SetUtilities.h"
#include "frc2/command/SubsystemBase.h"
@@ -26,7 +27,10 @@ class CommandTestBase : public ::testing::Test {
class TestSubsystem : public SubsystemBase {};
protected:
class MockCommand : public Command {
/**
* NOTE: Moving mock objects causes EXPECT_CALL to not work correctly!
*/
class MockCommand : public CommandHelper<CommandBase, MockCommand> {
public:
MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
MOCK_METHOD0(IsFinished, bool());
@@ -65,7 +69,7 @@ class CommandTestBase : public ::testing::Test {
.WillRepeatedly(::testing::Return(m_requirements));
}
MockCommand(const MockCommand& other) : Command{other} {}
MockCommand(const MockCommand& other) : CommandHelper{other} {}
void SetFinished(bool finished) {
EXPECT_CALL(*this, IsFinished())
@@ -77,11 +81,6 @@ class CommandTestBase : public ::testing::Test {
scheduler.Cancel(this);
}
protected:
std::unique_ptr<Command> TransferOwnership() && { // NOLINT
return std::make_unique<MockCommand>(std::move(*this));
}
private:
wpi::SmallSet<Subsystem*, 4> m_requirements;
};

36
wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp
View File

@@ -3,43 +3,51 @@
// the WPILib BSD license file in the root directory of this project.
#include "CommandTestBase.h"
#include "frc2/command/Command.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/RunCommand.h"
#include "gtest/gtest.h"
using namespace frc2;
class SchedulingRecursionTest : public CommandTestBaseWithParam<bool> {};
class SchedulingRecursionTest
: public CommandTestBaseWithParam<Command::InterruptionBehavior> {};
class SelfCancellingCommand
: public CommandHelper<CommandBase, SelfCancellingCommand> {
public:
SelfCancellingCommand(CommandScheduler* scheduler, Subsystem* requirement)
: m_scheduler(scheduler) {
SelfCancellingCommand(CommandScheduler* scheduler, Subsystem* requirement,
Command::InterruptionBehavior interruptionBehavior =
Command::InterruptionBehavior::kCancelSelf)
: m_scheduler(scheduler), m_interrupt(interruptionBehavior) {
AddRequirements(requirement);
}
void Initialize() override { m_scheduler->Cancel(this); }
InterruptionBehavior GetInterruptionBehavior() const override {
return m_interrupt;
}
private:
CommandScheduler* m_scheduler;
InterruptionBehavior m_interrupt;
};
/**
* Checks <a
* href="https://github.com/wpilibsuite/allwpilib/issues/4259">wpilibsuite/allwpilib#4259</a>.
*/
TEST_P(SchedulingRecursionTest, CancelFromInitialize) {
TEST_F(SchedulingRecursionTest, CancelFromInitialize) {
CommandScheduler scheduler = GetScheduler();
bool hasOtherRun = false;
TestSubsystem requirement;
SelfCancellingCommand selfCancels{&scheduler, &requirement};
auto selfCancels = SelfCancellingCommand(&scheduler, &requirement);
RunCommand other =
RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
scheduler.Schedule(GetParam(), &selfCancels);
scheduler.Schedule(&selfCancels);
scheduler.Run();
// interruptibility of new arrival isn't checked
scheduler.Schedule(&other);
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
@@ -48,16 +56,18 @@ TEST_P(SchedulingRecursionTest, CancelFromInitialize) {
EXPECT_TRUE(hasOtherRun);
}
TEST_P(SchedulingRecursionTest, DefaultCommand) {
TEST_P(SchedulingRecursionTest,
DefaultCommandGetsRescheduledAfterSelfCanceling) {
CommandScheduler scheduler = GetScheduler();
bool hasOtherRun = false;
TestSubsystem requirement;
SelfCancellingCommand selfCancels{&scheduler, &requirement};
auto selfCancels =
SelfCancellingCommand(&scheduler, &requirement, GetParam());
RunCommand other =
RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
scheduler.SetDefaultCommand(&requirement, std::move(other));
scheduler.Schedule(GetParam(), &selfCancels);
scheduler.Schedule(&selfCancels);
scheduler.Run();
scheduler.Run();
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
@@ -93,5 +103,7 @@ TEST_F(SchedulingRecursionTest, CancelFromEnd) {
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
}
INSTANTIATE_TEST_SUITE_P(SchedulingRecursionTests, SchedulingRecursionTest,
testing::Bool());
INSTANTIATE_TEST_SUITE_P(
SchedulingRecursionTests, SchedulingRecursionTest,
testing::Values(Command::InterruptionBehavior::kCancelSelf,
Command::InterruptionBehavior::kCancelIncoming));