Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[cmd3] Enforce command lifetimes across all opmode and command scopes (#8705)

Browse Source 
Commands are no longer able to outlive their schedule-site's scope,
regardless of how they were scheduled (set as a default command, bound
to a trigger, or manually scheduled)

As a consequence, default commands need better tracking so the default
command setting can be released when their scope exits and the next-most
appropriate default command can be rescheduled (eg, an opmode sets a
default command, then the globally-scoped default is restored when the
opmode exits). Some complexity is required here to make it work well for
edge cases.

Like `schedule()`, `setDefaultCommand()` will immediately start the new
default command if called inside of another command to avoid 1-loop
delays. However, this does not apply when called by the _current_
default command, as it would result in attempting to cancel the default
command while it's mounted (which is impossible and would throw an
exception)

```java
class Robot extends OpModeRobot {
  final Drive drive = new Drive();
  final CommandXboxController controller = new CommandXboxController(1);

  public Robot() {
    // global default command, active unless overridden in an opmode or command
    drive.setDefaultCommand(drive.stop());

    // global trigger binding, always active
    controller.rightBumper().onTrue(drive.setX());
  }
}

@Teleop
class ExampleOpMode extends PeriodicOpMode {
  public ExampleOpMode(Robot robot) {
    // opmode-specific default command
    robot.drive.setDefaultCommand(robot.drive.operatorControl(robot.controller));

    // opmode-specific binding
    robot.controller.leftBumper().whileTrue(robot.drive.stop());

    // opmode-specific binding that takes precedence over the global binding
    // because it happens last; it "wins out" over the `setX()` binding
    robot.controller.rightBumper().onTrue(robot.drive.selfTest());
  }

  @Override
  public void periodic() {
    Scheduler.getDefault().run();
  }
}
```
This commit is contained in:
Sam Carlberg
2026-04-09 20:05:42 -04:00
committed by GitHub
parent 5a96685c86
commit 02c6030251
23 changed files with 677 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
commandsv3/src/main/java/org/wpilib/command3/BindingScope.java
View File

@@ -17,20 +17,24 @@ interface BindingScope {
*/
boolean active();
static BindingScope global() {
return Global.INSTANCE;
}
/**
* Creates the narrowest binding scope available based on the current state of the scheduler and
* selected opmode.
*
* @param scheduler The scheduler to create the binding scope for.
* @return The narrowest binding scope available.
*/
static BindingScope createNarrowestScope(Scheduler scheduler) {
Command currentCommand = scheduler.currentCommand();
long currentOpMode = OpModeFetcher.getFetcher().getOpModeId();
static BindingScope forCommand(Scheduler scheduler, Command command) {
return new ForCommand(scheduler, command);
}
static BindingScope forOpmode(long opmodeId) {
if (opmodeId == 0) {
throw new IllegalArgumentException("Invalid OpMode ID provided");
if (currentCommand != null) {
return new ForCommand(scheduler, currentCommand);
} else if (currentOpMode != 0) {
return new ForOpmode(currentOpMode);
} else {
return Global.INSTANCE;
}
return new ForOpmode(opmodeId);
}
/** A global binding scope. Bindings in this scope are always active. */

6
commandsv3/src/main/java/org/wpilib/command3/BindingType.java
View File

@@ -42,5 +42,9 @@ enum BindingType {
* rising edge, it will be canceled then - unlike {@link #SCHEDULE_ON_FALLING_EDGE}, which would
* allow it to continue to run.
*/
RUN_WHILE_LOW
RUN_WHILE_LOW,
/** Continuously attempts to schedule a command as long as the signal remains high. */
CONTINUOUSLY_SCHEDULE_WHILE_HIGH,
/** Continuously attempts to schedule a command as long as the signal remains low. */
CONTINUOUSLY_SCHEDULE_WHILE_LOW
}

151
commandsv3/src/main/java/org/wpilib/command3/Scheduler.java
View File

@@ -95,7 +95,18 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
* protobuf serializer. However, it is up to the user to log those events themselves.
*/
public final class Scheduler implements ProtobufSerializable {
private final Map<Mechanism, Command> m_defaultCommands = new LinkedHashMap<>();
/**
* The default command bindings for each mechanism. Binding lists are ordered by priority; the
* last element in the list is the highest priority default command to be used. Bindings need to
* be periodically checked and removed when they're inactive.
*/
private final Map<Mechanism, List<Binding>> m_defaultCommandBindings = new LinkedHashMap<>();
/**
* All bindings attached to this scheduler. This lets us cancel commands tied to scopes that go
* inactive. Bindings need to be periodically checked and removed when they're inactive.
*/
private final Collection<Binding> m_activeBindings = new ArrayList<>();
/** The set of commands scheduled since the start of the previous run. */
private final SequencedSet<CommandState> m_queuedToRun = new LinkedHashSet<>();
@@ -164,14 +175,24 @@ public final class Scheduler implements ProtobufSerializable {
private Scheduler() {}
/**
* Sets the default command for a mechanism. The command must require that mechanism, and cannot
* require any other mechanisms.
* Sets the default command for a mechanism. The command must require that mechanism and cannot
* require any other mechanisms. If another default command has already been set for this
* mechanism, the one provided will supersede it.
*
* <p>If this is called inside a running opmode or in a running command, the default command
* setting will only apply while that opmode or command is active. When the opmode or command
* exits, the previous default command setting will be restored.
*
* <p>Commands running as default commands may call this method to change their mechanism's
* default command on the fly. The new default command will take effect at the end of the
* scheduler's loop cycle.
*
* @param mechanism the mechanism for which to set the default command
* @param defaultCommand the default command to execute on the mechanism
* @throws IllegalArgumentException if the command does not meet the requirements for being a
* default command
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) {
if (!defaultCommand.requires(mechanism)) {
throw new IllegalArgumentException(
@@ -183,17 +204,51 @@ public final class Scheduler implements ProtobufSerializable {
"A mechanism's default command cannot require other mechanisms");
}
m_defaultCommands.put(mechanism, defaultCommand);
var currentCommand = currentCommand();
BindingScope scope = BindingScope.createNarrowestScope(this);
var binding =
new Binding(
scope,
BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH,
defaultCommand,
new Throwable().getStackTrace());
var currentDefaultCommand = getDefaultCommandFor(mechanism);
m_defaultCommandBindings.computeIfAbsent(mechanism, k -> new ArrayList<>()).add(binding);
if (currentCommand != null && currentCommand != currentDefaultCommand) {
// User called `setDefaultCommand` inside another command.
// Immediately reprocess the default commands for this mechanism to ensure it's in sync with
// the rest of the commands in the scheduler. This is required because we normally schedule
// the default commands at the start of the scheduler `run()` method, so the new default
// command wouldn't be handled until the next run (ie, the previous default command would
// still be active for the current iteration)
//
// Note that we cannot do this if the current default command is the caller because commands
// cannot be canceled while mounted.
processDefaultCommands(mechanism);
}
}
/**
* Gets the default command set for a mechanism.
* Gets the default command currently used for a mechanism.
*
* @param mechanism The mechanism
* @return The default command, or null if no default command was ever set
*/
public Command getDefaultCommandFor(Mechanism mechanism) {
return m_defaultCommands.get(mechanism);
var bindings = m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
if (bindings.isEmpty()) {
return null;
}
return bindings.getLast().command();
}
// package-private helper for unit test access
List<Binding> getDefaultCommandBindingsFor(Mechanism mechanism) {
return m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
}
/**
@@ -275,17 +330,7 @@ public final class Scheduler implements ProtobufSerializable {
// This prevents commands from outliving the opmodes that scheduled them, or from outliving
// their parents (eg if someone writes a command that manually calls schedule(Command) instead
// of using triggers to do so).
Command currentCommand = currentCommand();
long currentOpmode = OpModeFetcher.getFetcher().getOpModeId();
BindingScope scope;
if (currentCommand != null) {
scope = BindingScope.forCommand(this, currentCommand);
} else if (currentOpmode != 0) {
scope = BindingScope.forOpmode(currentOpmode);
} else {
scope = BindingScope.global();
}
BindingScope scope = BindingScope.createNarrowestScope(this);
// Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
// stack frame filtering and modification.
@@ -319,6 +364,11 @@ public final class Scheduler implements ProtobufSerializable {
}
}
// Track this binding so we can disable it when it's out of scope.
// Note that, even though triggers can clean themselves up, commands that are manually scheduled
// cannot do the same, so we have to track them in the scheduler.
m_activeBindings.add(binding);
// Evict conflicting on-deck commands
// We check above if the input command is lower priority than any of these,
// so at this point we're guaranteed to be >= priority than anything already on deck
@@ -491,6 +541,8 @@ public final class Scheduler implements ProtobufSerializable {
* Updates the command scheduler. This will run operations in the following order:
*
* <ol>
* <li>Cancel any commands bound to scopes that have gone inactive, such as having been
* scheduled in an opmode that's no longer selected on the driverstation
* <li>Run sideloaded functions from {@link #sideload(Consumer)} and {@link
* #addPeriodic(Runnable)}
* <li>Update trigger bindings to queue and cancel bound commands
@@ -506,6 +558,9 @@ public final class Scheduler implements ProtobufSerializable {
public void run() {
final long startMicros = RobotController.getTime();
// Cancel any commands with stale binding scopes
cancelStaleBindings();
// Sideloads may change some state that affects triggers. Run them first.
runPeriodicSideloads();
@@ -526,6 +581,17 @@ public final class Scheduler implements ProtobufSerializable {
m_lastRunTimeMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds);
}
private void cancelStaleBindings() {
for (var iterator = m_activeBindings.iterator(); iterator.hasNext(); ) {
var binding = iterator.next();
if (binding.scope().active()) {
continue;
}
cancel(binding.command());
iterator.remove();
}
}
private void promoteScheduledCommands() {
// Clear any commands that conflict with the scheduled set
for (var queuedState : m_queuedToRun) {
@@ -696,18 +762,49 @@ public final class Scheduler implements ProtobufSerializable {
}
private void scheduleDefaultCommands() {
// Schedule the default commands for every mechanism that doesn't currently have a running or
// scheduled command.
m_defaultCommands.forEach(
(mechanism, defaultCommand) -> {
if (m_runningCommands.keySet().stream().noneMatch(c -> c.requires(mechanism))
&& m_queuedToRun.stream().noneMatch(c -> c.command().requires(mechanism))
&& defaultCommand != null) {
// Nothing currently running or scheduled
// Schedule the mechanism's default command, if it has one
schedule(defaultCommand);
m_defaultCommandBindings.keySet().forEach(this::processDefaultCommands);
}
private void processDefaultCommands(Mechanism mechanism) {
var bindings = m_defaultCommandBindings.get(mechanism);
// Remove default command bindings that are no longer active.
// If a default command is running when its scope goes inactive, also be sure to cancel it.
bindings.removeIf(
b -> {
if (!b.scope().active()) {
cancel(b.command());
return true;
}
return false;
});
if (bindings.isEmpty()) {
// Nothing to do. No active bindings remain.
return;
}
// Cancel any default command except the narrowest-scoped one (the last binding in the list)
for (int i = 0; i < bindings.size() - 1; i++) {
Command widerScopeDefaultCommand = bindings.get(i).command();
cancel(widerScopeDefaultCommand);
}
// Check if the mechanism is currently in use. We can queue the default command if it's not.
for (Command runningCommand : m_runningCommands.keySet()) {
if (runningCommand.requires(mechanism)) {
return;
}
}
for (CommandState queuedState : m_queuedToRun) {
if (queuedState.command().requires(mechanism)) {
return;
}
}
// Nothing currently running or queued that needs this mechanism. Queue the default command.
var defaultCommand = bindings.getLast();
schedule(defaultCommand);
}
/**

18
commandsv3/src/main/java/org/wpilib/command3/Trigger.java
View File

@@ -351,23 +351,7 @@ public class Trigger implements BooleanSupplier {
}
private void addBinding(BindingType bindingType, Command command) {
Command currentCommand = m_scheduler.currentCommand();
long currentOpmode = OpModeFetcher.getFetcher().getOpModeId();
BindingScope scope;
if (currentCommand != null) {
// A command is creating a binding - make it scoped to that specific command.
// The binding will be removed when the command exits.
scope = BindingScope.forCommand(m_scheduler, currentCommand);
} else if (currentOpmode != 0) {
// An opmode is currently running; scope the binding to just that mode.
// The binding will be removed when the opmode exits.
scope = BindingScope.forOpmode(currentOpmode);
} else {
// No opmode selected and no command is running; the binding is global in scope.
scope = BindingScope.global();
}
var scope = BindingScope.createNarrowestScope(m_scheduler);
addBinding(scope, bindingType, command);
}
}

78
commandsv3/src/test/java/org/wpilib/command3/BindingScopeTest.java Normal file
View File

@@ -0,0 +1,78 @@
// 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 org.wpilib.command3;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertInstanceOf;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertSame;
import org.junit.jupiter.api.Test;
class BindingScopeTest extends SchedulerTest {
@Test
void narrowestScopeInOpmode() {
m_opModeId = 1;
m_opModeName = "Opmode 1";
var scope = BindingScope.createNarrowestScope(m_scheduler);
assertInstanceOf(BindingScope.ForOpmode.class, scope);
assertEquals(m_opModeId, ((BindingScope.ForOpmode) scope).opmodeId());
}
@Test
void narrowestScopeInCommand() {
BindingScope[] scopeRef = new BindingScope[1];
var command =
Command.noRequirements()
.executing(
coroutine -> {
BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
scopeRef[0] = scope;
})
.named("Command 1");
m_scheduler.schedule(command);
m_scheduler.run();
assertNotNull(scopeRef[0], "Command did not execute");
assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
}
@Test
void narrowestScopeInOpmodeAndCommand() {
m_opModeId = 314;
m_opModeName = "Opmode pi";
BindingScope[] scopeRef = new BindingScope[1];
var command =
Command.noRequirements()
.executing(
coroutine -> {
BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
scopeRef[0] = scope;
})
.named("Command pi");
m_scheduler.schedule(command);
m_scheduler.run();
// Scope should be for the command, not the opmode
assertNotNull(scopeRef[0], "Command did not execute");
assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
}
@Test
void narrowestScopeWithoutOpmodeOrCommand() {
// Just to be explicit: an opmode ID of zero means no opmode is selected in the DS
m_opModeId = 0;
m_opModeName = null;
var scope = BindingScope.createNarrowestScope(m_scheduler);
assertSame(BindingScope.Global.INSTANCE, scope);
}
}

25
commandsv3/src/test/java/org/wpilib/command3/CommandTestBase.java
View File

@@ -6,12 +6,15 @@ package org.wpilib.command3;
import java.util.ArrayList;
import java.util.List;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.wpilib.system.RobotController;
class CommandTestBase {
protected Scheduler m_scheduler;
protected List<SchedulerEvent> m_events;
protected long m_opModeId = 0;
protected String m_opModeName = "";
@BeforeEach
void initScheduler() {
@@ -20,4 +23,26 @@ class CommandTestBase {
m_events = new ArrayList<>();
m_scheduler.addEventListener(m_events::add);
}
@BeforeEach
void initOpmodeFetcher() {
OpModeFetcher.setFetcher(
new OpModeFetcher() {
@Override
long getOpModeId() {
return m_opModeId;
}
@Override
String getOpModeName() {
return m_opModeName;
}
});
}
@AfterEach
void resetOpmodeFetcher() {
m_opModeId = 0;
m_opModeName = "";
}
}

5
commandsv3/src/test/java/org/wpilib/command3/SchedulerCancellationTests.java
View File

@@ -179,9 +179,14 @@ class SchedulerCancellationTests extends CommandTestBase {
// Then running should get it into the set of running commands
m_scheduler.run();
assertTrue(m_scheduler.isRunning(command));
for (Mechanism mechanism : mechanisms) {
assertEquals(List.of(command), m_scheduler.getRunningCommandsFor(mechanism));
}
// Canceling should clear out the set of running commands
m_scheduler.cancelAll();
assertFalse(m_scheduler.isRunning(command), "Command was not canceled by cancelAll()");
// Then ticking the scheduler once to fully remove the command and schedule the defaults
m_scheduler.run();

218
commandsv3/src/test/java/org/wpilib/command3/SchedulerDefaultCommandTests.java Normal file
View File

@@ -0,0 +1,218 @@
// 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 org.wpilib.command3;
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 java.util.List;
import org.junit.jupiter.api.Test;
class SchedulerDefaultCommandTests extends CommandTestBase {
@Test
void globalDefaultCommandIsAlwaysUsed() {
var mech = new Mechanism("Mech", m_scheduler);
var defaultCommand = mech.run(Coroutine::park).named("Custom Default Command");
mech.setDefaultCommand(defaultCommand);
assertFalse(m_scheduler.isScheduledOrRunning(defaultCommand));
m_scheduler.run();
assertTrue(m_scheduler.isRunning(defaultCommand));
m_opModeId = 1;
m_opModeName = "Opmode 1";
m_scheduler.run();
assertTrue(
m_scheduler.isRunning(defaultCommand),
"Default command should still be running when the opmode changes");
}
@Test
void defaultCommandSetInOpmodeStops() {
var mech = new Mechanism("Mech", m_scheduler);
var initialDefaultCommand = mech.idle();
mech.setDefaultCommand(initialDefaultCommand);
m_opModeId = 1;
m_opModeName = "Opmode 1";
var opModeScopedCommand = mech.run(Coroutine::park).named("Custom Default Command");
mech.setDefaultCommand(opModeScopedCommand);
m_scheduler.run();
assertTrue(m_scheduler.isRunning(opModeScopedCommand));
// Change opmode. The globally scoped default command should run
m_opModeId = 2;
m_opModeName = "Opmode 2";
m_scheduler.run();
assertFalse(m_scheduler.isRunning(opModeScopedCommand));
assertTrue(m_scheduler.isRunning(initialDefaultCommand));
}
@Test
void defaultCommandSetInCommandStops() {
var mech = new Mechanism("Mech", m_scheduler);
var initialDefaultCommand = mech.idle();
mech.setDefaultCommand(initialDefaultCommand);
var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
var scopingCommand =
Command.noRequirements()
.executing(
co -> {
mech.setDefaultCommand(commandScopedCommand);
co.park();
})
.named("Scoping Command");
m_scheduler.schedule(scopingCommand);
m_scheduler.run();
assertTrue(m_scheduler.isRunning(scopingCommand));
assertTrue(m_scheduler.isRunning(commandScopedCommand));
assertFalse(m_scheduler.isRunning(initialDefaultCommand));
m_scheduler.cancel(scopingCommand);
m_scheduler.run();
assertFalse(m_scheduler.isRunning(scopingCommand));
assertFalse(m_scheduler.isRunning(commandScopedCommand));
assertTrue(m_scheduler.isRunning(initialDefaultCommand));
}
@Test
void interruptingDefaultCommandInterruptsOwner() {
var mech = new Mechanism("Mech", m_scheduler);
var initialDefaultCommand = mech.idle();
mech.setDefaultCommand(initialDefaultCommand);
var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
var scopingCommand =
Command.noRequirements()
.executing(
co -> {
mech.setDefaultCommand(commandScopedCommand);
co.park();
})
.named("Scoping Command");
final Command interruptor = mech.run(Coroutine::park).named("Interruptor");
m_scheduler.schedule(scopingCommand);
m_scheduler.run();
assertTrue(m_scheduler.isRunning(scopingCommand));
assertTrue(m_scheduler.isRunning(commandScopedCommand));
assertFalse(m_scheduler.isRunning(initialDefaultCommand));
m_scheduler.schedule(interruptor);
m_scheduler.run();
assertFalse(m_scheduler.isRunning(scopingCommand));
assertFalse(m_scheduler.isRunning(commandScopedCommand));
assertFalse(m_scheduler.isRunning(initialDefaultCommand));
assertTrue(m_scheduler.isRunning(interruptor));
}
@Test
void defaultCommandStackup() {
var mech = new Mechanism("Mech", m_scheduler);
var initialDefaultCommand = mech.idle();
mech.setDefaultCommand(initialDefaultCommand);
var opModeScopedCommand = mech.run(Coroutine::park).named("Opmode Scoped Default Command");
var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
final Command scopingCommand =
Command.noRequirements()
.executing(
co -> {
mech.setDefaultCommand(commandScopedCommand);
co.park();
})
.named("Scoping Command");
m_opModeId = 1;
m_opModeName = "Opmode 1";
mech.setDefaultCommand(opModeScopedCommand);
m_scheduler.run();
assertTrue(
m_scheduler.isRunning(opModeScopedCommand),
"Opmode-scoped default command should be active");
m_scheduler.schedule(scopingCommand);
m_scheduler.run();
assertFalse(
m_scheduler.isRunning(opModeScopedCommand),
"Opmode-scoped default command should have stopped");
assertTrue(m_scheduler.isRunning(commandScopedCommand));
m_events.clear();
m_opModeId = 2;
m_opModeName = "Opmode 2";
m_scheduler.run();
assertFalse(
m_scheduler.isRunning(opModeScopedCommand),
"Opmode-scoped default command should have stopped when the opmode changed");
assertFalse(
m_scheduler.isRunning(scopingCommand),
"Running opmode-scoped command should have stopped when the opmode changed");
assertFalse(
m_scheduler.isRunning(commandScopedCommand),
"Command-scoped default command should have stopped when its parent command stopped");
assertTrue(
m_scheduler.isRunning(initialDefaultCommand), "Global default command should be active");
}
@Test
void defaultCommandChangingDefaultCommand() {
var mech =
new Mechanism("Mech", m_scheduler) {
Command makeCommand1() {
return run(co -> {
setDefaultCommand(makeCommand2());
// One-shot default commands won't appear in the `getRunningCommandsFor` list,
// We need to park the coroutine so we can observe the change
co.park();
})
.named("Command 1");
}
Command makeCommand2() {
return run(co -> {
setDefaultCommand(makeCommand1());
co.park();
})
.named("Command 2");
}
};
mech.setDefaultCommand(mech.makeCommand1());
// Command 1 and Command 2 should alternate as default commands
for (int i = 0; i < 10; i++) {
m_scheduler.run();
String expectedDefaultCommand = i % 2 == 0 ? "Command 1" : "Command 2";
assertEquals(
List.of(expectedDefaultCommand),
m_scheduler.getRunningCommandsFor(mech).stream().map(Command::name).toList());
}
// Check for memory leaks. If every call to `setDefaultCommand` were to add a new binding,
// we'd have 1 binding object per loop and quickly use a ton of memory
var defaultCommandBindings = m_scheduler.getDefaultCommandBindingsFor(mech);
assertEquals(
List.of("Mech[IDLE]", "Command 1", "Command 2", "Command 1"),
defaultCommandBindings.stream().map(b -> b.command().name()).toList());
m_scheduler.run();
assertEquals(
List.of("Mech[IDLE]", "Command 1", "Command 2"),
defaultCommandBindings.stream().map(b -> b.command().name()).toList());
}
}

79
design-docs/commands-v3.md
View File

@@ -175,6 +175,62 @@ the required mechanisms, priority level, last time to process, and total process
runs of the same command object have different ID numbers and processing time data. The total time
spent in the scheduler loop will also be included, but not the aggregate total of _all_ loops.
### Scoping
There are three possible scopes for commands and bindings. These scopes apply to triggers, default commands,
and manually scheduled commands.
1. The global scope, where a command is scheduled or trigger binding is created outside of any running opmode
2. An opmode scope, where the command is scheduled or trigger binding is created inside of an opmode
3. A command scope, where the command is scheduled or trigger binding is created inside of another command
A command scheduled in a certain scope, a trigger binding created in a certain scope, or a default command set in
a certain scope will only exist _within that scope_. Changing the default command for a mechanism within a custom opmode
will only be applied while that opmode is active; when the opmode exits, the default command specified in the robot
constructor (or mechanism constructor) will be reapplied.
```java
class Robot extends OpModeRobot {
final ExampleMechanism mechanism = new ExampleMechanism();
final CommandGamepad gamepad = new CommandGamepad(1);
public Robot() {
// The default command will be used whenever it's not overridden in an opmode or inside a command
mechanism.setDefaultCommand(mechanism.run(...).named("Global Default Command"));
}
}
class ExampleOpmode extends PeriodicOpMode {
public ExampleOpMode(Robot robot) {
// This overrides the default command set in the Robot constructor.
// Once this opmode exits, the global default command will be used again.
robot.mechanism.setDefaultCommand(robot.mechanism.run(...).named("Opmode Default Command"));
// This trigger binding will be cleaned up when the opmode exits.
// It will only be able to fire while this opmode is running.
// If the opmode exits and then restarts, the binding will be created anew when the opmode is reinstantiated.
robot.gamepad.leftBumper().onTrue(robot.mechanism.run(...).named("Bound Command"));
// A manually scheduled command in an opmode will be canceled when the opmode exits if it hasn't already exited on its own
Scheduler.getInstance().schedule(Command.noRequirements().executing(coroutine -> {
// Change the default command while this command is running.
// It will be reset to the opmode-scoped default command when this command exits.
// If no opmode-scoped default command exists, it will be reset to the global default command.
robot.mechanism.setDefaultCommand(...);
// This trigger binding will only be active while this command is running.
// It will be unbound and garbage collected when this command exits.
robot.gamepad.rightBumper().onTrue(...);
}).named("Scoping Command"));
}
@Override
public void periodic() {
Scheduler.getDefault().run();
}
}
```
## Non-Goals
### Preemptive Multitasking
@@ -352,14 +408,27 @@ conflict with those in the queue are cancelled, then the queued commands are pro
Scheduling an inner command will bypass the queue and immediately begin to run. This avoids delays
caused by loop timings for deeply nested commands.
### Command scopes
Trigger bindings, default command settings, and manually scheduled commands all have a _scope_ in
which they are valid. When a scope exits, all bindings that were created in that scope are removed,
any mechanisms with default commands set during that scope have their default commands reset to the
default command set in the enclosing scope, and all commands scheduled in that scope are canceled.
Scopes are represented by `BindingScope`, and commands bound to a scope are tracked with a `Binding`
object. `BindingScope` is a sealed interface with three implementations: `Global` (the always-active
scope, eg `Robot` class initialization), `OpMode` (a scope while a particular opmode is running),
and `Command` (a scope for a specific running command).
### Scheduler `run()` Cycle
1. Run periodic sideload functions
2. Poll the event loop for triggers (which may queue or cancel commands)
3. Schedule default commands for the next iteration to pick up and start running
4. Promote scheduled commands to running
1. Cancel commands bound to inactive scopes
2. Run periodic sideload functions
3. Poll the event loop for triggers (which may queue or cancel commands)
4. Schedule default commands for the next iteration to pick up and start running
5. Promote scheduled commands to running
1. Cancels any running commands that conflict with scheduled ones
5. Iterate over running commands
6. Iterate over running commands
1. Mount
2. Run until yield point is reached or an error is raised
3. Unmount

18
wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp
View File

@@ -159,6 +159,7 @@ struct Instance {
// Op mode lookup
wpi::util::mutex opModeMutex;
wpi::util::DenseMap<int64_t, HAL_OpModeOption> opModes;
bool userProgramStarted = false;
wpi::units::second_t nextMessageTime = 0_s;
@@ -679,7 +680,24 @@ void DriverStation::ClearOpModes() {
HAL_SetOpModeOptions(nullptr, 0);
}
void DriverStation::ObserveUserProgramStarting() {
::GetInstance().userProgramStarted = true;
HAL_ObserveUserProgramStarting();
}
int64_t DriverStation::GetOpModeId() {
if (!::GetInstance().userProgramStarted) {
return 0;
}
return GetControlWord().GetOpModeId();
}
std::string DriverStation::GetOpMode() {
if (!::GetInstance().userProgramStarted) {
return 0;
}
return GetInstance().OpModeToString(GetOpModeId());
}

11
wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp
View File

@@ -117,13 +117,14 @@ void OpModeRobotBase::StartCompetition() {
// Wait for new data from the driver station, with 50 ms timeout
HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status);
// Call HAL_ObserveUserProgramStarting() here as a one-shot to ensure it is
// called after the notifier alarm is set. The notifier alarm is set using
// relative time, so tests that wait on the user program to start and then
// step time won't work correctly if we call this before setting the alarm.
// Call DriverStation::ObserveUserProgramStarting() here as a one-shot to
// ensure it is called after the notifier alarm is set. The notifier alarm
// is set using relative time, so tests that wait on the user program to
// start and then step time won't work correctly if we call this before
// setting the alarm.
if (!calledObserveUserProgramStarting) {
calledObserveUserProgramStarting = true;
HAL_ObserveUserProgramStarting();
DriverStation::ObserveUserProgramStarting();
}
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);

4
wpilibc/src/main/native/cpp/framework/TimedRobot.cpp
View File

@@ -9,7 +9,7 @@
#include <cstdio>
#include <utility>
#include "wpi/hal/DriverStation.h"
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/UsageReporting.hpp"
#include "wpi/system/Errors.hpp"
@@ -23,7 +23,7 @@ void TimedRobot::StartCompetition() {
// Tell the DS that the robot is ready to be enabled
std::puts("\n********** Robot program startup complete **********");
HAL_ObserveUserProgramStarting();
DriverStation::ObserveUserProgramStarting();
// Loop forever, calling the appropriate mode-dependent function
while (true) {

16
wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp
View File

@@ -454,6 +454,20 @@ class DriverStation final {
*/
static void ClearOpModes();
/**
* Sets the program starting flag in the DS. This will also allow
* getOpModeId() and getOpMode() to return values for the selected
* OpMode in the DS application, if the DS is connected by the time this
* method is called.
*
* <p>Most users will not need to use this method; the TimedRobot and
* OpModeRobot robot framework classes will call it automatically after
* the main robot class is instantiated.
*
* <p>This is what changes the DS to showing robot code ready.
*/
static void ObserveUserProgramStarting();
/**
* Gets the operating mode selected on the driver station. Note this does not
* mean the robot is enabled; use IsEnabled() for that. In a match, this will
@@ -464,7 +478,7 @@ class DriverStation final {
* @return the unique ID provided by the AddOpMode() function; may return 0 or
* a unique ID not added, so callers should be prepared to handle that case
*/
static int64_t GetOpModeId() { return GetControlWord().GetOpModeId(); }
static int64_t GetOpModeId();
/**
* Gets the operating mode selected on the driver station. Note this does not

1
wpilibc/src/main/python/semiwrap/DriverStation.yml
View File

@@ -74,6 +74,7 @@ classes:
GetControlWord:
GetStickTouchpadFinger:
GetStickTouchpadFingerAvailable:
ObserveUserProgramStarting:
wpi::DriverStation::TouchpadFinger:
attributes:
down:

2
wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
View File

@@ -31,7 +31,7 @@ void Robot::StartCompetition() {
wpi::DriverStation::ProvideRefreshedDataEventHandle(event.GetHandle());
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
wpi::DriverStation::ObserveUserProgramStarting();
while (!m_exit) {
modeThread.InControl(wpi::DriverStation::GetControlWord());

43
wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java
View File

@@ -16,6 +16,8 @@ import org.wpilib.datalog.FloatArrayLogEntry;
import org.wpilib.datalog.IntegerArrayLogEntry;
import org.wpilib.datalog.StringLogEntry;
import org.wpilib.datalog.StructLogEntry;
import org.wpilib.framework.OpModeRobot;
import org.wpilib.framework.TimedRobot;
import org.wpilib.hardware.hal.AllianceStationID;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
@@ -578,6 +580,7 @@ public final class DriverStation {
private static boolean m_silenceJoystickWarning;
private static boolean m_userProgramStarted = false;
private static final Map<Long, OpModeOption> m_opModes = new HashMap<>();
private static final ReentrantLock m_opModesMutex = new ReentrantLock();
@@ -1469,16 +1472,47 @@ public final class DriverStation {
}
}
/**
* Sets the program starting flag in the DS. This will also allow {@link #getOpModeId()} and
* {@link #getOpMode()} to return values for the selected OpMode in the DS application, if the DS
* is connected by the time this method is called.
*
* <p>Most users will not need to use this method; the {@link TimedRobot} and {@link OpModeRobot}
* robot framework classes will call it automatically after the main robot class is instantiated.
* However, teams using the commandsv3 library and a custom main robot class need to be careful to
* only call this method after all mechanisms and global trigger bindings are set up. If not, any
* setup performed in the main robot class may be incorrectly bound to the opmode selected in the
* DS if it's connected by the time the robot program boots up.
*
* <p>This is what changes the DS to showing robot code ready.
*
* @see #getOpMode()
* @see #getOpModeId()
*/
public static void observeUserProgramStarting() {
m_userProgramStarted = true;
DriverStationJNI.observeUserProgramStarting();
}
/**
* Gets the operating mode selected on the driver station. Note this does not mean the robot is
* enabled; use isEnabled() for that. In a match, this will indicate the operating mode selected
* for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
* auto period ends, this will change to reflect the operating mode selected for teleop.
*
* <p>This method always returns {@code 0} while the main robot class is being constructed and
* initialized (more specifically, it returns {@code 0} until {@link
* #observeUserProgramStarting()} is called, which the WPILib framework will automatically call
* during {@link TimedRobot#startCompetition()} and {@link OpModeRobot#startCompetition()}).
*
* @return the unique ID provided by the addOpMode() function; may return 0 or a unique ID not
* added, so callers should be prepared to handle that case
*/
public static long getOpModeId() {
if (!m_userProgramStarted) {
return 0;
}
m_cacheDataMutex.lock();
try {
return m_controlWord.getOpModeId();
@@ -1493,10 +1527,19 @@ public final class DriverStation {
* for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
* auto period ends, this will change to reflect the operating mode selected for teleop.
*
* <p>This method always returns an empty string {@code ""} while the main robot class is being
* constructed and initialized (more specifically, it returns {@code ""} until {@link
* #observeUserProgramStarting()} is called, which the WPILib framework will automatically call
* during {@link TimedRobot#startCompetition()} and {@link OpModeRobot#startCompetition()}).
*
* @return Operating mode string; may return a string not in the list of options, so callers
* should be prepared to handle that case
*/
public static String getOpMode() {
if (!m_userProgramStarted) {
return "";
}
m_cacheDataMutex.lock();
try {
return m_opMode;

2
wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java
View File

@@ -617,7 +617,7 @@ public abstract class OpModeRobot extends RobotBase {
// this before setting the alarm.
if (!calledObserveUserProgramStarting) {
calledObserveUserProgramStarting = true;
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
}
try {

4
wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java
View File

@@ -7,7 +7,7 @@ package org.wpilib.framework;
import static org.wpilib.units.Units.Seconds;
import java.util.PriorityQueue;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.system.RobotController;
@@ -128,7 +128,7 @@ public class TimedRobot extends IterativeRobotBase {
// Tell the DS that the robot is ready to be enabled
System.out.println("********** Robot program startup complete **********");
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
// Loop forever, calling the appropriate mode-dependent function
while (true) {

38
wpilibj/src/test/java/org/wpilib/driverstation/DriverStationTest.java
View File

@@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.params.provider.Arguments.arguments;
import java.util.stream.Stream;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
@@ -26,6 +28,34 @@ class DriverStationTest {
assertEquals(expected, DriverStation.isJoystickConnected(1));
}
@Test
void getOpmodeIdReturnsZeroUntilUserProgramStarts() {
DriverStationSim.setOpMode(0x1234);
DriverStationSim.notifyNewData();
assertEquals(0, DriverStation.getOpModeId());
DriverStation.observeUserProgramStarting();
// need to manually mask because the upper eight bits include robot mode information
assertEquals(0x1234, DriverStation.getOpModeId() & 0xFFFF);
}
@Test
void getOpmodeReturnsEmptyStringUntilUserProgramStarts() {
DriverStationSim.setOpMode(0x1234);
DriverStationSim.notifyNewData();
assertEquals("", DriverStation.getOpMode());
DriverStation.observeUserProgramStarting();
// in Sim, the opmode string is just the stringified version of the opmode i64 "<0000...0000>"
// we need to parse the string to get the
// need to manually mask because the upper eight bits include robot mode information
String opmodeName = DriverStation.getOpMode();
assertEquals(
"0x1234",
String.format(
"0x%x", Long.parseLong(opmodeName.substring(1, opmodeName.length() - 1)) & 0xFFFF));
}
static Stream<Arguments> isConnectedProvider() {
return Stream.of(
arguments(0, 0, 0, false),
@@ -52,4 +82,12 @@ class DriverStationTest {
arguments(true, true, false),
arguments(true, false, false));
}
@AfterEach
@SuppressWarnings("PMD.AvoidAccessibilityAlteration")
void resetUserProgramFlag() throws ReflectiveOperationException {
var field = DriverStation.class.getDeclaredField("m_userProgramStarted");
field.setAccessible(true);
field.set(null, false);
}
}

3
wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java
View File

@@ -7,7 +7,6 @@ package org.wpilib.templates.educational;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.framework.RobotBase;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.internal.DriverStationModeThread;
import org.wpilib.util.WPIUtilJNI;
@@ -50,7 +49,7 @@ public class EducationalRobot extends RobotBase {
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
DriverStation.refreshControlWordFromCache(word);

3
wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java
View File

@@ -7,7 +7,6 @@ package org.wpilib.templates.robotbaseskeleton;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.framework.RobotBase;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.internal.DriverStationModeThread;
import org.wpilib.util.WPIUtilJNI;
@@ -45,7 +44,7 @@ public class Robot extends RobotBase {
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
DriverStation.refreshControlWordFromCache(word);

3
wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java
View File

@@ -7,7 +7,6 @@ package org.wpilib.templates.romieducational;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.framework.RobotBase;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.internal.DriverStationModeThread;
import org.wpilib.util.WPIUtilJNI;
@@ -50,7 +49,7 @@ public class EducationalRobot extends RobotBase {
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
DriverStation.refreshControlWordFromCache(word);

3
wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java
View File

@@ -7,7 +7,6 @@ package org.wpilib.templates.xrpeducational;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.framework.RobotBase;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.internal.DriverStationModeThread;
import org.wpilib.util.WPIUtilJNI;
@@ -50,7 +49,7 @@ public class EducationalRobot extends RobotBase {
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
DriverStationJNI.observeUserProgramStarting();
DriverStation.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
DriverStation.refreshControlWordFromCache(word);