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

[commands] Add Commands v3 framework (#6518)

Browse Source 
The framework fundamentally relies on the continuation API added in Java 21 (which is currently internal to the JDK). Continuations allow for call stacks to be saved to the heap and resumed later.

The async framework allows command bodies to be written in an imperative style. However, an async command will need to be actively cooperative and periodically call coroutine.yield() in loops to yield control back to the command scheduler to let it process other commands.

There are also some other additions like priority levels (as opposed to a blanket yes/no for ignoring incoming commands), factories requiring names be provided for commands, and the scheduler tracking all running commands and not just the highest-level groups. However, those changes aren't unique to an async framework, and could just as easily be used in a traditional command framework.
This commit is contained in:
Sam Carlberg
2025-10-10 16:47:22 -04:00
committed by GitHub
parent 33f91589b4
commit b37e2d9343
77 changed files with 12259 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
commandsv3/src/main/java/org/wpilib/commands3/Binding.java Normal file
View File

@@ -0,0 +1,26 @@
// 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.commands3;
import edu.wpi.first.util.ErrorMessages;
/**
* A single trigger binding.
*
* @param scope The scope in which the binding is active.
* @param type The type of binding; or, when the bound command should run
* @param command The bound command. Cannot be null.
* @param frames The stack frames when the binding was created. Used for telemetry and error
* reporting so if a command throws an exception, we can tell users where that command was bound
* instead of giving a fairly useless backtrace of the command framework.
*/
record Binding(BindingScope scope, BindingType type, Command command, StackTraceElement[] frames) {
public Binding {
ErrorMessages.requireNonNullParam(scope, "scope", "Binding");
ErrorMessages.requireNonNullParam(type, "type", "Binding");
ErrorMessages.requireNonNullParam(command, "command", "Binding");
ErrorMessages.requireNonNullParam(frames, "frames", "Binding");
}
}

52
commandsv3/src/main/java/org/wpilib/commands3/BindingScope.java Normal file
View File

@@ -0,0 +1,52 @@
// 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.commands3;
/**
* A scope for when a binding is live. Bindings tied to a scope must be deleted when the scope
* becomes inactive.
*/
@SuppressWarnings("PMD.ImplicitFunctionalInterface")
interface BindingScope {
/**
* Checks if the scope is active. Bindings for inactive scopes are removed from the scheduler.
*
* @return True if the scope is still active, false if not.
*/
boolean active();
static BindingScope global() {
return Global.INSTANCE;
}
static BindingScope forCommand(Scheduler scheduler, Command command) {
return new ForCommand(scheduler, command);
}
/** A global binding scope. Bindings in this scope are always active. */
final class Global implements BindingScope {
// No reason not to be a singleton.
public static final Global INSTANCE = new Global();
@Override
public boolean active() {
return true;
}
}
/**
* A binding scoped to the lifetime of a specific command. This should be used when a binding is
* created within a command, tying the lifetime of the binding to the declaring command.
*
* @param scheduler The scheduler managing the command.
* @param command The command being scoped to.
*/
record ForCommand(Scheduler scheduler, Command command) implements BindingScope {
@Override
public boolean active() {
return scheduler.isRunning(command);
}
}
}

46
commandsv3/src/main/java/org/wpilib/commands3/BindingType.java Normal file
View File

@@ -0,0 +1,46 @@
// 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.commands3;
/** Describes when a command bound to a trigger should run. */
enum BindingType {
/**
* An immediate or manual binding created when calling {@link Scheduler#schedule(Command)}
* directly, without using a trigger to bind it to a signal.
*/
IMMEDIATE,
/**
* Schedules (forks) a command on a rising edge signal. The command will run until it completes or
* is interrupted by another command requiring the same mechanisms.
*/
SCHEDULE_ON_RISING_EDGE,
/**
* Schedules (forks) a command on a falling edge signal. The command will run until it completes
* or is interrupted by another command requiring the same mechanisms.
*/
SCHEDULE_ON_FALLING_EDGE,
/**
* Schedules (forks) a command on a rising edge signal. If the command is still running on the
* next rising edge, it will be canceled then; otherwise, it will be scheduled again.
*/
TOGGLE_ON_RISING_EDGE,
/**
* Schedules (forks) a command on a falling edge signal. If the command is still running on the
* next falling edge, it will be canceled then; otherwise, it will be scheduled again.
*/
TOGGLE_ON_FALLING_EDGE,
/**
* Schedules a command on a rising edge signal. If the command is still running on the next
* falling edge, it will be canceled then - unlike {@link #SCHEDULE_ON_RISING_EDGE}, which would
* allow it to continue to run.
*/
RUN_WHILE_HIGH,
/**
* Schedules a command on a falling edge signal. If the command is still running on the next
* rising edge, it will be canceled then - unlike {@link #SCHEDULE_ON_FALLING_EDGE}, which would
* allow it to continue to run.
*/
RUN_WHILE_LOW
}

416
commandsv3/src/main/java/org/wpilib/commands3/Command.java Normal file
View File

@@ -0,0 +1,416 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.units.measure.Time;
import java.util.Collection;
import java.util.Collections;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import org.wpilib.annotation.NoDiscard;
/**
* Performs some task using one or more {@link Mechanism mechanisms}. Commands are fundamentally
* backed by a {@link Coroutine} that can be used to write imperative code that runs asynchronously.
*
* <p>Programmers familiar with the earlier versions of the command framework can think of a v3
* command similar to a v1 or v2 command that executes the lifecycle methods in a single method, as
* demonstrated in the example below. (Note, however, that more sophisticated code than this is
* possible!
*
* <pre>{@code
* coroutine -> {
* initialize();
* while (!isFinished()) {
* execute();
* coroutine.yield(); // be sure to yield at the end of the loop
* }
* end();
* }
* }</pre>
*
* <p><strong>Note:</strong> Because coroutines are <i>opt-in</i> collaborate constructs, every
* command implementation <strong>must</strong> call {@link Coroutine#yield()} within any periodic
* loop. Failure to do so may result in an unrecoverable infinite loop.
*
* <h2>Requirements</h2>
*
* <p>Commands require zero or more mechanisms. To prevent conflicting control requests from running
* simultaneously (for example, commanding an elevator to both raise and lower at the same time), a
* running command has <i>exclusive ownership</i> of all of its required mechanisms. If another
* command with an equal or greater {@link #priority()} is scheduled that requires one or more of
* those same mechanisms, it will interrupt and cancel the running command.
*
* <p>The recommended way to create a command is using {@link Mechanism#run(Consumer)} or a related
* factory method to create commands that require a single mechanism (for example, a command that
* drives an elevator up and down or rotates an arm). Commands may be <i>composed</i> into {@link
* ParallelGroup parallel groups} and {@link SequentialGroup sequences} to build more complex
* behavior out of fundamental building blocks. These built-in compositions will require every
* mechanism used by every command in them, even if those commands aren't always running, and thus
* can leave certain required mechanisms in an <i>uncommanded</i> state: owned, but not used, this
* can lead to mechanisms sagging under gravity or running the previous motor control request they
* were given.
*
* <h2>Advanced Usage</h2>
*
* <p>For example, a hypothetical drive-and-score sequence could be coded in two ways: one with a
* sequence chain, or one that uses the lower-level coroutine API. Coroutines can be used in an
* async/await style that you may be familiar with from languages like JavaScript, Python, or C#
* (note that there is no {@code async} keyword; commands are inherently asynchronous). Nested
* commands may be forked and awaited, but will not outlive the command that forked them; there is
* no analog for something like a {@code ScheduleCommand} from the v2 commands framework.
*
* <pre>{@code
* class Robot extends TimedRobot {
* private final Drivetrain drivetrain = new Drivetrain();
* private final Elevator elevator = new Elevator();
* private final Gripper gripper = new Gripper();
*
* // Basic sequence builder - owns all 3 mechanisms for the full duration,
* // even when they're not being used
* private Command basicScoringSequence() {
* return drivetrain.driveToScoringLocation()
* .andThen(elevator.moveToScoringHeight())
* .andThen(gripper.release())
* .named("Scoring Sequence (Basic)");
* }
*
* // Advanced sequence with async/await - only owns mechanisms while they're
* // being used, allowing default commands or other user-triggered commands
* // to run when not in use. Interrupting one of the inner commands while it's
* // running will cancel the entire sequence.
* private Command advancedScoringSequence() {
* return Command.noRequirements().executing(coroutine -> {
* coroutine.await(drivetrain.driveToScoringLocation());
* coroutine.await(elevator.moveToScoringHeight());
* coroutine.await(gripper.release());
* }).named("Scoring Sequence (Advanced)");
* }
* }
* }</pre>
*/
@NoDiscard("Commands must be used! Did you mean to fork it or bind it to a trigger?")
public interface Command {
/** The default command priority. */
int DEFAULT_PRIORITY = 0;
/**
* The lowest possible command priority. Commands with the lowest priority can be interrupted by
* any other command, including other minimum-priority commands.
*/
int LOWEST_PRIORITY = Integer.MIN_VALUE;
/**
* The highest possible command priority. Commands with the highest priority can only be
* interrupted by other maximum-priority commands.
*/
int HIGHEST_PRIORITY = Integer.MAX_VALUE;
/**
* Runs the command. Commands that need to periodically run until a goal state is reached should
* simply run a while loop like {@code while (!atGoal()) { ... } } and call {@link
* Coroutine#yield()} at the end of the loop.
*
* <p><strong>Warning:</strong> any loops in a command must call {@code coroutine.yield()}.
* Failure to do so will prevent anything else in your robot code from running. Commands are
* <i>opt-in</i> collaborative constructs; don't be greedy!
*
* @param coroutine the coroutine backing the command's execution
*/
void run(Coroutine coroutine);
/**
* An optional lifecycle hook that can be implemented to execute some code whenever this command
* is canceled before it naturally completes. Commands should be careful to do a single-shot
* cleanup (for example, setting a motor to zero volts) and not do any complex looping logic here.
*/
default void onCancel() {
// NOP by default
}
/**
* The name of the command.
*
* @return the name of the command
*/
@NoDiscard
String name();
/**
* The mechanisms required by the command. This is used by the scheduler to determine if two
* commands conflict with each other. No mechanism may be required by more than one running
* command at a time.
*
* @return the set of mechanisms required by the command
*/
@NoDiscard
Set<Mechanism> requirements();
/**
* The priority of the command. If a command is scheduled that conflicts with another running or
* pending command, their priority values are compared to determine which should run. If the
* scheduled command is lower priority than the running command, then it will not be scheduled and
* the running command will continue to run. If it is the same or higher priority, then the
* running command will be canceled and the scheduled command will start to run.
*
* @return the priority of the command
*/
@NoDiscard
default int priority() {
return DEFAULT_PRIORITY;
}
/**
* Checks if this command has a lower {@link #priority() priority} than another command.
*
* @param other the command to compare with
* @return true if this command has a lower priority than the other one, false otherwise
*/
@NoDiscard
default boolean isLowerPriorityThan(Command other) {
requireNonNullParam(other, "other", "Command.isLowerPriorityThan");
return priority() < other.priority();
}
/**
* Checks if this command requires a particular mechanism.
*
* @param mechanism the mechanism to check
* @return true if the mechanism is required, false if not
*/
@NoDiscard
default boolean requires(Mechanism mechanism) {
return requirements().contains(mechanism);
}
/**
* Checks if this command conflicts with another command.
*
* @param other the commands to check against
* @return true if both commands require at least one of the same mechanism, false if both
* commands have completely different requirements
*/
@NoDiscard
default boolean conflictsWith(Command other) {
requireNonNullParam(other, "other", "Command.conflictsWith");
return !Collections.disjoint(requirements(), other.requirements());
}
/**
* Creates a new command that runs this one for a maximum duration, after which it is forcibly
* canceled. This is particularly useful for autonomous routines where you want to prevent your
* entire autonomous period spent stuck on a single action because a mechanism doesn't quite reach
* its setpoint (for example, spinning up a flywheel or driving to a particular location on the
* field). The resulting command will have the same name as this one, with the timeout period
* appended.
*
* @param timeout the maximum duration that the command is permitted to run. Negative or zero
* values will result in the command running only once before being canceled.
* @return the timed out command.
*/
default Command withTimeout(Time timeout) {
requireNonNullParam(timeout, "timeout", "Command.withTimeout");
return race(this, waitFor(timeout).named("Timeout: " + timeout.toLongString()))
.named(name() + " [" + timeout.toLongString() + " timeout]");
}
/**
* Creates a command that does not require any hardware; that is, it does not affect the state of
* any physical objects. This is useful for commands that do some cleanup or state management,
* such as resetting odometry or sensors, that you don't want to interrupt a command that's
* controlling the mechanisms it affects.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link StagedCommandBuilder} for details.
*
* @return a builder that can be used to configure the resulting command
*/
static NeedsExecutionBuilderStage noRequirements() {
return new StagedCommandBuilder().noRequirements();
}
/**
* Creates a command that requires one or more mechanisms.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link StagedCommandBuilder} for details.
*
* @param requirement The first required mechanism
* @param rest Any other required mechanisms
* @return A command builder
*/
static NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... rest) {
// parameters will be null checked by the builder
return new StagedCommandBuilder().requiring(requirement, rest);
}
/**
* Creates command that requires some number of mechanisms.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link StagedCommandBuilder} for details.
*
* @param requirements The required mechanisms. May be empty, but cannot contain null values.
* @return A command builder
*/
static NeedsExecutionBuilderStage requiring(Collection<Mechanism> requirements) {
// parameters will be null checked by the builder
return new StagedCommandBuilder().requiring(requirements);
}
/**
* Starts creating a command that runs a group of multiple commands in parallel. The command will
* complete when every command in the group has completed naturally.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link ParallelGroupBuilder} for details.
*
* @param commands The commands to run in parallel
* @return A command builder
*/
static ParallelGroupBuilder parallel(Command... commands) {
// parameters will be null checked by the builder
return new ParallelGroupBuilder().requiring(commands);
}
/**
* Starts creating a command that runs a group of multiple commands in parallel. The command will
* complete when any command in the group has completed naturally; all other commands in the group
* will be canceled.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link ParallelGroupBuilder} for details.
*
* @param commands The commands to run in parallel
* @return A command builder
*/
static ParallelGroupBuilder race(Command... commands) {
// parameters will be null checked by the builder
return new ParallelGroupBuilder().optional(commands);
}
/**
* Starts creating a command that runs a group of multiple commands in sequence. The command will
* complete when the last command in the group has completed naturally. Commands in the group will
* run in the order they're passed to this method.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link SequentialGroupBuilder} for details.
*
* @param commands The commands to run in sequence.
* @return A command builder
*/
static SequentialGroupBuilder sequence(Command... commands) {
// parameters will be null checked by the builder
return new SequentialGroupBuilder().andThen(commands);
}
/**
* Starts creating a command that simply waits for some condition to be met. The command will not
* require any mechanisms.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link StagedCommandBuilder} for details.
*
* @param condition The condition to wait for
* @return A command builder
*/
static NeedsNameBuilderStage waitUntil(BooleanSupplier condition) {
requireNonNullParam(condition, "condition", "Command.waitUntil");
return noRequirements().executing(coroutine -> coroutine.waitUntil(condition));
}
/**
* Creates a command that simply waits for a given duration. The command will not require any
* mechanisms.
*
* @param duration How long to wait
* @return A command builder
*/
static NeedsNameBuilderStage waitFor(Time duration) {
requireNonNullParam(duration, "duration", "Command.waitFor");
return noRequirements().executing(coroutine -> coroutine.wait(duration));
}
/**
* Creates a command that runs this one and ends when the end condition is met (if this command
* has not already exited by then).
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link ParallelGroupBuilder} for details.
*
* @param endCondition The end condition to wait for.
* @return The waiting command
*/
default ParallelGroupBuilder until(BooleanSupplier endCondition) {
requireNonNullParam(endCondition, "endCondition", "Command.until");
return new ParallelGroupBuilder()
.optional(this, Command.waitUntil(endCondition).named("Until Condition"));
}
/**
* Creates a command sequence, starting from this command and then running the next one. More
* commands can be added with the builder before naming and creating the sequence.
*
* <pre>{@code
* Sequence aThenBThenC =
* commandA()
* .andThen(commandB())
* .andThen(commandC())
* .withAutomaticName();
* }</pre>
*
* @param next The command to run after this one in the sequence
* @return A sequence builder
*/
default SequentialGroupBuilder andThen(Command next) {
// parameter will be null checked by the builder
return new SequentialGroupBuilder().andThen(this).andThen(next);
}
/**
* Creates a parallel command group, running this command alongside one or more other commands.
* The group will exit once every command has finished.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link ParallelGroupBuilder} for details.
*
* <pre>{@code
* ParallelGroup abc =
* commandA()
* .alongWith(commandB(), commandC())
* .withAutomaticName();
* }</pre>
*
* @param parallel The commands to run in parallel with this one
* @return A parallel group builder
*/
default ParallelGroupBuilder alongWith(Command... parallel) {
return new ParallelGroupBuilder().requiring(this).requiring(parallel);
}
/**
* Creates a parallel command group, running this command alongside one or more other commands.
* The group will exit after any command finishes.
*
* <p>More configuration options are needed after calling this function before the command can be
* created. See {@link ParallelGroupBuilder} for details.
*
* @param parallel The commands to run in parallel with this one
* @return A parallel group builder
*/
default ParallelGroupBuilder raceWith(Command... parallel) {
return new ParallelGroupBuilder().optional(this).optional(parallel);
}
}

100
commandsv3/src/main/java/org/wpilib/commands3/CommandState.java Normal file
View File

@@ -0,0 +1,100 @@
// 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.commands3;
import static java.util.Objects.requireNonNull;
/** Represents the state of a command as it is executed by the scheduler. */
final class CommandState {
// Two billion unique IDs should be enough for anybody.
private static int s_lastId = 0;
private final Command m_command;
private final Command m_parent;
private final Coroutine m_coroutine;
private final Binding m_binding;
private double m_lastRuntimeMs = -1;
private double m_totalRuntimeMs;
private final int m_id;
/**
* Creates a new command state object.
*
* @param command The command being tracked.
* @param parent The parent command composition that scheduled the tracked command. Null if the
* command was scheduled by a top level construct like trigger bindings or manually scheduled
* by a user. For deeply nested compositions, this tracks the <i>direct parent command</i>
* that invoked the schedule() call; in this manner, an ancestry tree can be built, where each
* {@code CommandState} object references a parent node in the tree.
* @param coroutine The coroutine to which the command is bound.
* @param binding The binding that caused the command to be scheduled.
*/
CommandState(Command command, Command parent, Coroutine coroutine, Binding binding) {
m_command = requireNonNull(command, "WPILib bug: command state given null command");
m_parent = parent; // allowed to be null
m_coroutine = requireNonNull(coroutine, "WPILib bug: command state given null coroutine");
m_binding = requireNonNull(binding, "WPILib bug: command state given null binding");
// This is incredibly non-thread safe, but nobody should be using the command framework across
// multiple threads anyway. Worst case scenario, we'll get duplicate IDs for commands scheduled
// by different threads and cause bad telemetry data without affecting program correctness.
m_id = ++s_lastId;
}
public Command command() {
return m_command;
}
public Command parent() {
return m_parent;
}
public Coroutine coroutine() {
return m_coroutine;
}
public Binding binding() {
return m_binding;
}
/**
* How long the command took to run the last time it executed. Defaults to -1 if the command has
* not run at least once.
*
* @return The runtime, in milliseconds.
*/
public double lastRuntimeMs() {
return m_lastRuntimeMs;
}
public void setLastRuntimeMs(double lastRuntimeMs) {
m_lastRuntimeMs = lastRuntimeMs;
m_totalRuntimeMs += lastRuntimeMs;
}
public double totalRuntimeMs() {
return m_totalRuntimeMs;
}
public int id() {
return m_id;
}
@Override
public boolean equals(Object obj) {
return obj instanceof CommandState that && m_id == that.m_id;
}
@Override
public int hashCode() {
return m_id;
}
@Override
public String toString() {
return "CommandState[command=%s, parent=%s, coroutine=%s, id=%d]"
.formatted(m_command, m_parent, m_coroutine, m_id);
}
}

73
commandsv3/src/main/java/org/wpilib/commands3/CommandTraceHelper.java Normal file
View File

@@ -0,0 +1,73 @@
// 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.commands3;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Stream;
final class CommandTraceHelper {
private CommandTraceHelper() {
// Utility class
}
/**
* Creates a modified stack trace where the trace of the scheduling code replaces the trace of the
* internal scheduler logic.
*
* @param commandExceptionTrace The trace of the exception raised during command execution.
* @param commandScheduleTrace The trace of when the command was scheduled.
* @return A new array of stack trace elements.
*/
public static StackTraceElement[] modifyTrace(
StackTraceElement[] commandExceptionTrace, StackTraceElement[] commandScheduleTrace) {
List<StackTraceElement> frames = new ArrayList<>();
List<String> filteredClasses =
List.of(
Coroutine.class.getName(),
Continuation.class.getName(),
Scheduler.class.getName(),
"org.wpilib.commands3.StagedCommandBuilder$BuilderBackedCommand",
"jdk.internal.vm.Continuation");
boolean sawRun = false;
for (var exceptionFrame : commandExceptionTrace) {
if (!filteredClasses.contains(exceptionFrame.getClassName())) {
frames.add(exceptionFrame);
}
// Inject the schedule trace immediately after the line of user code that called Scheduler.run
if (sawRun) {
// Inject a marker frame just so there's a distinction between the command's code and the
// code that scheduled it, since they occur asynchronously
frames.add(new StackTraceElement("=== Command Binding Trace ===", "", "", -1));
// Drop internal trigger frames, since they're not helpful for users.
// The first frame here should be the line of user code that bound the command to a trigger.
Stream.of(commandScheduleTrace)
.dropWhile(
frame -> {
boolean inTriggerInternals = frame.getClassName().equals(Trigger.class.getName());
boolean isScheduleCall =
frame.getClassName().equals(Scheduler.class.getName())
&& "schedule".equals(frame.getMethodName());
return inTriggerInternals || isScheduleCall;
})
.filter(frame -> !filteredClasses.contains(frame.getClassName()))
.forEach(frames::add);
break;
}
if (exceptionFrame.getClassName().equals(Scheduler.class.getName())
&& "run".equals(exceptionFrame.getMethodName())) {
sawRun = true;
}
}
return frames.toArray(StackTraceElement[]::new);
}
}

122
commandsv3/src/main/java/org/wpilib/commands3/ConflictDetector.java Normal file
View File

@@ -0,0 +1,122 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Collectors;
/** Utility class for helping with detecting conflicts between commands. */
public final class ConflictDetector {
private ConflictDetector() {
// This is a utility class!
}
/**
* A conflict between two commands.
*
* @param a The first conflicting command.
* @param b The second conflicting command.
* @param sharedRequirements The set of mechanisms required by both commands. This set is
* read-only
*/
public record Conflict(Command a, Command b, Set<Mechanism> sharedRequirements) {
/**
* Gets a descriptive message for the conflict. The description includes the names of the
* conflicting commands and the names of all mechanisms required by both commands.
*
* @return A description of the conflict.
*/
public String description() {
var shared =
sharedRequirements.stream()
.map(Mechanism::getName)
.sorted()
.collect(Collectors.joining(", "));
return "%s and %s both require %s".formatted(a.name(), b.name(), shared);
}
}
/**
* Validates that a set of commands have no internal requirement conflicts. An error is thrown if
* a conflict is detected.
*
* @param commands The commands to validate
* @throws IllegalArgumentException If at least one pair of commands is found in the input where
* both commands have at least one required mechanism in common
*/
public static void throwIfConflicts(Collection<? extends Command> commands) {
requireNonNullParam(commands, "commands", "ConflictDetector.throwIfConflicts");
var conflicts = findAllConflicts(commands);
if (conflicts.isEmpty()) {
// No conflicts, all good
return;
}
StringBuilder message =
new StringBuilder("Commands running in parallel cannot share requirements: ");
for (int i = 0; i < conflicts.size(); i++) {
Conflict conflict = conflicts.get(i);
message.append(conflict.description());
if (i < conflicts.size() - 1) {
message.append("; ");
}
}
throw new IllegalArgumentException(message.toString());
}
/**
* Finds all conflicting pairs of commands in the input collection.
*
* @param commands The commands to check.
* @return All detected conflicts. The returned list is empty if no conflicts were found.
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public static List<Conflict> findAllConflicts(Collection<? extends Command> commands) {
requireNonNullParam(commands, "commands", "ConflictDetector.findAllConflicts");
List<Conflict> conflicts = new ArrayList<>();
int i = 0;
for (Command command : commands) {
i++;
int j = 0;
for (Command other : commands) {
j++;
if (j <= i) {
// Skip all elements in 0..i so the inner loop only checks elements from i + 1 onward.
// Ordering of the elements in the pair doesn't matter, and this prevents finding every
// pair twice eg (A, B) and (B, A).
continue;
}
if (command == other) {
// Commands cannot conflict with themselves, so just in case the input collection happens
// to have duplicate elements we just skip any pairs of a command with itself.
continue;
}
if (command.conflictsWith(other)) {
conflicts.add(findConflict(command, other));
}
}
}
return conflicts;
}
private static Conflict findConflict(Command a, Command b) {
Set<Mechanism> sharedRequirements = new HashSet<>(a.requirements());
sharedRequirements.retainAll(b.requirements());
return new Conflict(a, b, Set.copyOf(sharedRequirements));
}
}

230
commandsv3/src/main/java/org/wpilib/commands3/Continuation.java Normal file
View File

@@ -0,0 +1,230 @@
// 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.commands3;
import java.lang.invoke.MethodHandle;
import java.lang.invoke.MethodHandles;
import java.lang.invoke.MethodType;
import java.lang.invoke.WrongMethodTypeException;
/**
* A wrapper around the JDK internal Continuation class. Continuations are one-shot (i.e., not
* reusable after completion) and allow stack frames to be paused and resumed at a later time. They
* are the underpinning for virtual threads, which have their own scheduler and JVM support. Bare
* continuations are designed for internal use by the JVM; we have forcibly opened access to them
* for use by the commands framework due to limitations of virtual threads; notably, their complete
* lack of determinism and timing, which are critically important for real-time systems like robots.
*
* <p><strong>ONLY USE CONTINUATIONS IN A SINGLE THREADED CONTEXT.</strong> The JVM and JIT
* compilers make fundamental assumptions about how continuations are used, and rely on the code
* that uses it (which was intended to be virtual threads) to use it safely. <strong>Failure to use
* this API safely can result in JIT compilers to issue invalid code causing buggy behavior and JVM
* crashes at any time, up to and including on a field during an official match.</strong>
*
* <p>Teams don't need to use continuations directly with the commands framework; all mounting and
* unmounting is handled by the command scheduler and a coroutine wrapper.
*/
final class Continuation {
// The underlying jdk.internal.vm.Continuation object
// https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/Continuation.java
private final Object m_continuation;
static final Class<?> jdk_internal_vm_Continuation;
private static final MethodHandle CONSTRUCTOR;
@SuppressWarnings("PMD.AvoidFieldNameMatchingMethodName")
private static final MethodHandle YIELD;
@SuppressWarnings("PMD.AvoidFieldNameMatchingMethodName")
private static final MethodHandle RUN;
private static final MethodHandle IS_DONE;
private static final MethodHandle java_lang_thread_setContinuation;
private static final ThreadLocal<Continuation> mountedContinuation = new ThreadLocal<>();
static {
try {
jdk_internal_vm_Continuation = Class.forName("jdk.internal.vm.Continuation");
var lookup =
MethodHandles.privateLookupIn(jdk_internal_vm_Continuation, MethodHandles.lookup());
CONSTRUCTOR =
lookup.findConstructor(
jdk_internal_vm_Continuation,
MethodType.methodType(
void.class, ContinuationScope.jdk_internal_vm_ContinuationScope, Runnable.class));
YIELD =
lookup.findStatic(
jdk_internal_vm_Continuation,
"yield",
MethodType.methodType(
boolean.class, ContinuationScope.jdk_internal_vm_ContinuationScope));
RUN =
lookup.findVirtual(
jdk_internal_vm_Continuation, "run", MethodType.methodType(void.class));
IS_DONE =
lookup.findVirtual(
jdk_internal_vm_Continuation, "isDone", MethodType.methodType(boolean.class));
} catch (Throwable t) {
throw new ExceptionInInitializerError(t);
}
}
static {
try {
var lookup = MethodHandles.privateLookupIn(Thread.class, MethodHandles.lookup());
java_lang_thread_setContinuation =
lookup.findVirtual(
Thread.class,
"setContinuation",
MethodType.methodType(void.class, Continuation.jdk_internal_vm_Continuation));
} catch (Throwable t) {
throw new ExceptionInInitializerError(t);
}
}
/**
* Used to wrap any checked exceptions bubbled from when calling the internal continuation methods
* via reflection. Per the Continuation API as of Java 21, none of the methods we're calling will
* throw unchecked exceptions (IllegalState or other runtime exceptions, yes, and we bubble those
* up directly); however, the MethodHandle API's `invoke` method has `throws Throwable` in its
* signature and we have to handle it.
*/
@SuppressWarnings("PMD.DoNotExtendJavaLangError")
static final class InternalContinuationError extends Error {
InternalContinuationError(String message, Throwable cause) {
super(message, cause);
}
}
private final ContinuationScope m_scope;
/**
* Constructs a continuation.
*
* @param scope the continuation's scope, used in yield
* @param target the continuation's body
*/
@SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"})
Continuation(ContinuationScope scope, Runnable target) {
try {
m_continuation = CONSTRUCTOR.invoke(scope.m_continuationScope, target);
m_scope = scope;
} catch (RuntimeException | Error e) {
throw e;
} catch (Throwable t) {
throw new RuntimeException(t);
}
}
/**
* Suspends the current continuations up to this continuation's scope.
*
* @return {@code true} for success; {@code false} for failure
* @throws IllegalStateException if not currently in this continuation's scope
*/
@SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"})
public boolean yield() {
try {
return (boolean) YIELD.invoke(m_scope.m_continuationScope);
} catch (RuntimeException | Error e) {
throw e;
} catch (Throwable t) {
throw new InternalContinuationError(
"Continuation.yield() encountered an unexpected error", t);
}
}
/**
* Mounts and runs the continuation body until the body calls {@link #yield()}. If the
* continuation is suspended, it will continue from the most recent yield point.
*/
@SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"})
public void run() {
try {
RUN.invoke(m_continuation);
} catch (WrongMethodTypeException | ClassCastException e) {
throw new IllegalStateException("Unable to run the underlying continuation!", e);
} catch (RuntimeException | Error e) {
// The bound task threw an exception; re-throw it
throw e;
} catch (Throwable t) {
throw new InternalContinuationError("Continuation.run() encountered an unexpected error", t);
}
}
/**
* Tests whether this continuation is completed.
*
* @return whether this continuation is completed
*/
@SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"})
public boolean isDone() {
try {
return (boolean) IS_DONE.invoke(m_continuation);
} catch (RuntimeException | Error e) {
throw e;
} catch (Throwable t) {
throw new InternalContinuationError(
"Continuation.isDone() encountered an unexpected error", t);
}
}
/**
* Mounds a continuation to the current thread. Accepts null for clearing the currently mounted
* continuation.
*
* @param continuation the continuation to mount
*/
@SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"})
public static void mountContinuation(Continuation continuation) {
try {
if (continuation == null) {
java_lang_thread_setContinuation.invoke(Thread.currentThread(), null);
mountedContinuation.remove();
} else {
java_lang_thread_setContinuation.invoke(
Thread.currentThread(), continuation.m_continuation);
mountedContinuation.set(continuation);
}
} catch (RuntimeException | Error e) {
throw e;
} catch (Throwable t) {
// `t` is anything thrown internally by Thread.setContinuation.
// It only assigns to a field, no way to throw
// However, if the invocation fails for some reason, we'll end up with an
// IllegalStateException when attempting to run an unmounted continuation
throw new InternalContinuationError(
"Continuation.mountContinuation() encountered an unexpected error", t);
}
}
/**
* Gets the currently mounted continuation. This is thread-local value; calling this method on two
* different threads will give two different results.
*
* @return The continuation mounted on the current thread.
*/
public static Continuation getMountedContinuation() {
return mountedContinuation.get();
}
@Override
public String toString() {
return m_continuation.toString();
}
@SuppressWarnings("PMD.CompareObjectsWithEquals")
boolean isMounted() {
return this == getMountedContinuation();
}
}

54
commandsv3/src/main/java/org/wpilib/commands3/ContinuationScope.java Normal file
View File

@@ -0,0 +1,54 @@
// 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.commands3;
import java.lang.invoke.MethodHandle;
import java.lang.invoke.MethodHandles;
import java.lang.invoke.MethodType;
import java.util.Objects;
/** A continuation scope. */
final class ContinuationScope {
// The underlying jdk.internal.vm.ContinuationScope object
// https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/ContinuationScope.java
final Object m_continuationScope;
static final Class<?> jdk_internal_vm_ContinuationScope;
private static final MethodHandle CONSTRUCTOR;
static {
try {
jdk_internal_vm_ContinuationScope = Class.forName("jdk.internal.vm.ContinuationScope");
var lookup =
MethodHandles.privateLookupIn(jdk_internal_vm_ContinuationScope, MethodHandles.lookup());
CONSTRUCTOR =
lookup.findConstructor(
jdk_internal_vm_ContinuationScope, MethodType.methodType(void.class, String.class));
} catch (Throwable t) {
throw new ExceptionInInitializerError(t);
}
}
/**
* Constructs a new scope.
*
* @param name The scope's name
*/
ContinuationScope(String name) {
Objects.requireNonNull(name);
try {
m_continuationScope = CONSTRUCTOR.invoke(name);
} catch (Throwable e) {
throw new RuntimeException(e);
}
}
@Override
public String toString() {
return m_continuationScope.toString();
}
}

377
commandsv3/src/main/java/org/wpilib/commands3/Coroutine.java Normal file
View File

@@ -0,0 +1,377 @@
// 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.commands3;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.wpilibj.Timer;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
/**
* A coroutine object is injected into command's {@link Command#run(Coroutine)} method to allow
* commands to yield and compositions to run other commands. Commands are considered <i>bound</i> to
* a coroutine while they're scheduled; attempting to use a coroutine outside the command bound to
* it will result in an {@code IllegalStateException} being thrown.
*/
public final class Coroutine {
private final Scheduler m_scheduler;
private final Continuation m_backingContinuation;
/**
* Creates a new coroutine. Package-private; only the scheduler should be creating these.
*
* @param scheduler The scheduler running the coroutine
* @param scope The continuation scope the coroutine's backing continuation runs in
* @param callback The callback for the continuation to execute when mounted. Often a command
* function's body.
*/
Coroutine(Scheduler scheduler, ContinuationScope scope, Consumer<Coroutine> callback) {
m_scheduler = scheduler;
m_backingContinuation = new Continuation(scope, () -> callback.accept(this));
}
/**
* Yields control back to the scheduler to allow other commands to execute. This can be thought of
* as "pausing" the currently executing command.
*
* @return true
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public boolean yield() {
requireMounted();
try {
return m_backingContinuation.yield();
} catch (IllegalStateException e) {
if ("Pinned: MONITOR".equals(e.getMessage())) {
// Raised when a continuation yields inside a synchronized block or method:
// https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/Continuation.java#L396-L402
// Note: Not a thing in Java 24+
// Rethrow with an error message that's more helpful for our users
throw new IllegalStateException(
"Coroutine.yield() cannot be called inside a synchronized block or method. "
+ "Consider using a Lock instead of synchronized, "
+ "or rewrite your code to avoid locks and mutexes altogether.",
e);
} else {
// rethrow
throw e;
}
}
}
/**
* Parks the current command. No code in a command declared after calling {@code park()} will be
* executed. A parked command will never complete naturally and must be interrupted or canceled.
*
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
@SuppressWarnings("InfiniteLoopStatement")
public void park() {
requireMounted();
while (true) {
// 'this' is required because 'yield' is a semi-keyword and needs to be qualified
this.yield();
}
}
/**
* Schedules a child command and then immediately returns. The child command will run until its
* natural completion, the parent command exits, or the parent command cancels it.
*
* <p>This is a nonblocking operation. To fork and then wait for the child command to complete,
* use {@link #await(Command)}.
*
* <p>The parent command will continue executing while the child command runs, and can resync with
* the child command using {@link #await(Command)}.
*
* <pre>{@code
* Command example() {
* return Command.noRequirements().executing(coroutine -> {
* Command child = ...;
* coroutine.fork(child);
* // ... do more things
* // then sync back up with the child command
* coroutine.await(child);
* }).named("Example");
* }
* }</pre>
*
* <p>Note: forking a command that conflicts with a higher-priority command will fail. The forked
* command will not be scheduled, and the existing command will continue to run.
*
* @param commands The commands to fork.
* @throws IllegalStateException if called anywhere other than the coroutine's running command
* @see #await(Command)
*/
public void fork(Command... commands) {
requireMounted();
requireNonNullParam(commands, "commands", "Coroutine.fork");
for (int i = 0; i < commands.length; i++) {
requireNonNullParam(commands[i], "commands[" + i + "]", "Coroutine.fork");
}
// Check for user error; there's no reason to fork conflicting commands simultaneously
ConflictDetector.throwIfConflicts(List.of(commands));
// Shorthand; this is handy for user-defined compositions
for (var command : commands) {
m_scheduler.schedule(command);
}
}
/**
* Forks off some commands. Each command will run until its natural completion, the parent command
* exits, or the parent command cancels it. The parent command will continue executing while the
* forked commands run, and can resync with the forked commands using {@link
* #awaitAll(Collection)}.
*
* <pre>{@code
* Command example() {
* return Command.noRequirements().executing(coroutine -> {
* Collection<Command> innerCommands = ...;
* coroutine.fork(innerCommands);
* // ... do more things
* // then sync back up with the inner commands
* coroutine.awaitAll(innerCommands);
* }).named("Example");
* }
* }</pre>
*
* <p>Note: forking a command that conflicts with a higher-priority command will fail. The forked
* command will not be scheduled, and the existing command will continue to run.
*
* @param commands The commands to fork.
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void fork(Collection<? extends Command> commands) {
fork(commands.toArray(Command[]::new));
}
/**
* Awaits completion of a command. If the command is not currently scheduled or running, it will
* be scheduled automatically. This is a blocking operation and will not return until the command
* completes or has been interrupted by another command scheduled by the same parent.
*
* @param command the command to await
* @throws IllegalStateException if called anywhere other than the coroutine's running command
* @see #fork(Command...)
*/
public void await(Command command) {
requireMounted();
requireNonNullParam(command, "command", "Coroutine.await");
m_scheduler.schedule(command);
while (m_scheduler.isScheduledOrRunning(command)) {
// If the command is a one-shot, then the schedule call will completely execute the command.
// There would be nothing to await
this.yield();
}
}
/**
* Awaits completion of all given commands. If any command is not current scheduled or running, it
* will be scheduled.
*
* @param commands the commands to await
* @throws IllegalArgumentException if any of the commands conflict with each other
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void awaitAll(Collection<? extends Command> commands) {
requireMounted();
requireNonNullParam(commands, "commands", "Coroutine.awaitAll");
int i = 0;
for (Command command : commands) {
requireNonNullParam(command, "commands[" + i + "]", "Coroutine.awaitAll");
i++;
}
ConflictDetector.throwIfConflicts(commands);
for (var command : commands) {
m_scheduler.schedule(command);
}
while (commands.stream().anyMatch(m_scheduler::isScheduledOrRunning)) {
this.yield();
}
}
/**
* Awaits completion of all given commands. If any command is not current scheduled or running, it
* will be scheduled.
*
* @param commands the commands to await
* @throws IllegalArgumentException if any of the commands conflict with each other
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void awaitAll(Command... commands) {
awaitAll(Arrays.asList(commands));
}
/**
* Awaits completion of any given commands. Any command that's not already scheduled or running
* will be scheduled. After any of the given commands completes, the rest will be canceled.
*
* @param commands the commands to await
* @throws IllegalArgumentException if any of the commands conflict with each other
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void awaitAny(Collection<? extends Command> commands) {
requireMounted();
requireNonNullParam(commands, "commands", "Coroutine.awaitAny");
int i = 0;
for (Command command : commands) {
requireNonNullParam(command, "commands[" + i + "]", "Coroutine.awaitAny");
i++;
}
ConflictDetector.throwIfConflicts(commands);
// Schedule anything that's not already queued or running
for (var command : commands) {
m_scheduler.schedule(command);
}
while (commands.stream().allMatch(m_scheduler::isScheduledOrRunning)) {
this.yield();
}
// At least one command exited; cancel the rest.
commands.forEach(m_scheduler::cancel);
}
/**
* Awaits completion of any given commands. Any command that's not already scheduled or running
* will be scheduled. After any of the given commands completes, the rest will be canceled.
*
* @param commands the commands to await
* @throws IllegalArgumentException if any of the commands conflict with each other
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void awaitAny(Command... commands) {
awaitAny(Arrays.asList(commands));
}
/**
* Waits for some duration of time to elapse. Returns immediately if the given duration is zero or
* negative. Call this within a command or command composition to introduce a simple delay.
*
* <p>For example, a basic autonomous routine that drives straight for 5 seconds:
*
* <pre>{@code
* Command timedDrive() {
* return drivebase.run(coroutine -> {
* drivebase.tankDrive(1, 1);
* coroutine.wait(Seconds.of(5));
* drivebase.stop();
* }).named("Timed Drive");
* }
* }</pre>
*
* <p>Note that the resolution of the wait period is equal to the period at which {@link
* Scheduler#run()} is called by the robot program. If using a 20 millisecond update period, the
* wait will be rounded up to the nearest 20 millisecond interval; in this scenario, calling
* {@code wait(Milliseconds.of(1))} and {@code wait(Milliseconds.of(19))} would have identical
* effects.
*
* <p>Very small loop times near the loop period are sensitive to the order in which commands are
* executed. If a command waits for 10 ms at the end of a scheduler cycle, and then all the
* commands that ran before it complete or exit, and then the next cycle starts immediately, the
* wait will be evaluated at the <i>start</i> of that next cycle, which occurred less than 10 ms
* later. Therefore, the wait will see not enough time has passed and only exit after an
* additional cycle elapses, adding an unexpected extra 20 ms to the wait time. This becomes less
* of a problem with smaller loop periods, as the extra 1-loop delay becomes smaller.
*
* @param duration the duration of time to wait
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void wait(Time duration) {
requireMounted();
requireNonNullParam(duration, "duration", "Coroutine.wait");
var timer = new Timer();
timer.start();
while (!timer.hasElapsed(duration.in(Seconds))) {
this.yield();
}
}
/**
* Yields until a condition is met.
*
* @param condition The condition to wait for
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public void waitUntil(BooleanSupplier condition) {
requireMounted();
requireNonNullParam(condition, "condition", "Coroutine.waitUntil");
while (!condition.getAsBoolean()) {
this.yield();
}
}
/**
* Advanced users only: this permits access to the backing command scheduler to run custom logic
* not provided by the standard coroutine methods. Any commands manually scheduled through this
* will be canceled when the parent command exits - it's not possible to "fork" a command that
* lives longer than the command that scheduled it.
*
* @return the command scheduler backing this coroutine
* @throws IllegalStateException if called anywhere other than the coroutine's running command
*/
public Scheduler scheduler() {
requireMounted();
return m_scheduler;
}
private boolean isMounted() {
return m_backingContinuation.isMounted();
}
private void requireMounted() {
// Note: attempting to yield() outside a command will already throw an error due to the
// continuation being unmounted, but other actions like forking and awaiting should also
// throw errors. For consistent messaging, we use this helper in all places, not just the
// ones that interact with the backing continuation.
if (isMounted()) {
return;
}
throw new IllegalStateException("Coroutines can only be used by the command bound to them");
}
// Package-private for interaction with the scheduler.
// These functions are not intended for team use.
void runToYieldPoint() {
m_backingContinuation.run();
}
void mount() {
Continuation.mountContinuation(m_backingContinuation);
}
boolean isDone() {
return m_backingContinuation.isDone();
}
}

165
commandsv3/src/main/java/org/wpilib/commands3/Mechanism.java Normal file
View File

@@ -0,0 +1,165 @@
// 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.commands3;
import edu.wpi.first.units.measure.Time;
import java.util.List;
import java.util.function.Consumer;
import org.wpilib.annotation.NoDiscard;
/**
* Generic base class to represent mechanisms on a robot. Commands can require sole ownership of a
* mechanism; when a command that requires a mechanism is running, no other commands may use it at
* the same time.
*
* <p>Even though this class is named "Mechanism", it may be used to represent other physical
* hardware on a robot that should be controlled with commands - for example, an LED strip or a
* vision processor that can switch between different pipelines could be represented as mechanisms.
*/
public class Mechanism {
private final String m_name;
private final Scheduler m_registeredScheduler;
/**
* Creates a new mechanism registered with the default scheduler instance and named using the name
* of the class. Intended to be used by subclasses to get sane defaults without needing to
* manually declare a constructor.
*/
@SuppressWarnings("this-escape")
protected Mechanism() {
m_name = getClass().getSimpleName();
m_registeredScheduler = Scheduler.getDefault();
setDefaultCommand(idle());
}
/**
* Creates a new mechanism, registered with the default scheduler instance.
*
* @param name The name of the mechanism. Cannot be null.
*/
public Mechanism(String name) {
this(name, Scheduler.getDefault());
}
/**
* Creates a new mechanism, registered with the given scheduler instance.
*
* @param name The name of the mechanism. Cannot be null.
* @param scheduler The registered scheduler. Cannot be null.
*/
@SuppressWarnings("this-escape")
public Mechanism(String name, Scheduler scheduler) {
m_name = name;
m_registeredScheduler = scheduler;
setDefaultCommand(idle());
}
/**
* Gets the name of this mechanism.
*
* @return The name of the mechanism.
*/
@NoDiscard
public String getName() {
return m_name;
}
/**
* Sets the default command to run on the mechanism when no other command is scheduled. The
* default command's priority is effectively the minimum allowable priority for any command
* requiring a mechanism. For this reason, it's recommended that a default command have a priority
* less than {@link Command#DEFAULT_PRIORITY} so it doesn't prevent low-priority commands from
* running.
*
* <p>The default command is initially an idle command that only owns the mechanism without doing
* anything. This command has the lowest possible priority to allow any other command to run.
*
* @param defaultCommand the new default command
*/
public void setDefaultCommand(Command defaultCommand) {
m_registeredScheduler.setDefaultCommand(this, defaultCommand);
}
/**
* Gets the default command that was set by the latest call to {@link
* #setDefaultCommand(Command)}.
*
* @return The currently configured default command
*/
public Command getDefaultCommand() {
return m_registeredScheduler.getDefaultCommandFor(this);
}
/**
* Starts building a command that requires this mechanism.
*
* @param commandBody The main function body of the command.
* @return The command builder, for further configuration.
*/
public NeedsNameBuilderStage run(Consumer<Coroutine> commandBody) {
return new StagedCommandBuilder().requiring(this).executing(commandBody);
}
/**
* Starts building a command that requires this mechanism. The given function will be called
* repeatedly in an infinite loop. Useful for building commands that don't need state or multiple
* stages of logic.
*
* @param loopBody The body of the infinite loop.
* @return The command builder, for further configuration.
*/
public NeedsNameBuilderStage runRepeatedly(Runnable loopBody) {
return run(
coroutine -> {
while (true) {
loopBody.run();
coroutine.yield();
}
});
}
/**
* Returns a command that idles this mechanism until another command claims it. The idle command
* has {@link Command#LOWEST_PRIORITY the lowest priority} and can be interrupted by any other
* command.
*
* <p>The {@link #getDefaultCommand() default command} for every mechanism is an idle command
* unless a different default command has been configured.
*
* @return A new idle command.
*/
public Command idle() {
return run(Coroutine::park).withPriority(Command.LOWEST_PRIORITY).named(getName() + "[IDLE]");
}
/**
* Returns a command that idles this mechanism for the given duration of time.
*
* @param duration How long the mechanism should idle for.
* @return A new idle command.
*/
public Command idleFor(Time duration) {
return idle().withTimeout(duration);
}
/**
* Gets all running commands that require this mechanism. Commands are returned in the order in
* which they were scheduled. The returned list is read-only. Every command in the list will have
* been scheduled by the previous entry in the list or by intermediate commands that do not
* require the mechanism.
*
* @return The currently running commands that require the mechanism.
*/
@NoDiscard
public List<Command> getRunningCommands() {
return m_registeredScheduler.getRunningCommandsFor(this);
}
@Override
public String toString() {
return m_name;
}
}

56
commandsv3/src/main/java/org/wpilib/commands3/NeedsExecutionBuilderStage.java Normal file
View File

@@ -0,0 +1,56 @@
// 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.commands3;
import java.util.Collection;
import java.util.function.Consumer;
import org.wpilib.annotation.NoDiscard;
/**
* A stage in a command builder where requirements have already been specified and execution details
* are required. The next stage is a {@link NeedsNameBuilderStage} from {@link
* #executing(Consumer)}. Additional requirements may still be added before moving on to the next
* stage.
*/
@NoDiscard
public interface NeedsExecutionBuilderStage {
/**
* Adds a required mechanism for the command.
*
* @param requirement A required mechanism. Cannot be null.
* @return This builder object, for chaining
* @throws NullPointerException If {@code requirement} is null
*/
NeedsExecutionBuilderStage requiring(Mechanism requirement);
/**
* Adds one or more required mechanisms for the command.
*
* @param requirement A required mechanism. Cannot be null.
* @param extra Any extra required mechanisms. May be empty, but cannot contain null values.
* @return This builder object, for chaining
* @throws NullPointerException If {@code requirement} is null or {@code extra} contains a null
* value
*/
NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra);
/**
* Adds required mechanisms for the command.
*
* @param requirements Any required mechanisms. May be empty, but cannot contain null values.
* @return This builder object, for chaining
* @throws NullPointerException If {@code requirements} is null or contains a null value.
*/
NeedsExecutionBuilderStage requiring(Collection<Mechanism> requirements);
/**
* Sets the function body of the executing command.
*
* @param impl The command's body. Cannot be null.
* @return A builder for the next stage of command construction.
* @throws NullPointerException If {@code impl} is null.
*/
NeedsNameBuilderStage executing(Consumer<Coroutine> impl);
}

56
commandsv3/src/main/java/org/wpilib/commands3/NeedsNameBuilderStage.java Normal file
View File

@@ -0,0 +1,56 @@
// 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.commands3;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import org.wpilib.annotation.NoDiscard;
/**
* A stage in a command builder where requirements and main command execution logic have been set.
* No more changes to requirements or command implementation may happen after this point. This is
* the final step in command creation
*/
@NoDiscard
public interface NeedsNameBuilderStage {
/**
* Optionally sets a callback to execute when the command is canceled. The callback will not run
* if the command was canceled after being scheduled but before starting to run, and will not run
* if the command completes naturally or from encountering an unhandled exception.
*
* @param onCancel The function to execute when the command is canceled while running. May be
* null.
* @return This builder object, for chaining
*/
NeedsNameBuilderStage whenCanceled(Runnable onCancel);
/**
* Sets the priority level of the command.
*
* @param priority The desired priority level.
* @return This builder object, for chaining.
*/
NeedsNameBuilderStage withPriority(int priority);
/**
* Optionally sets an end condition for the command. If the end condition returns {@code true}
* before the command body set in {@link NeedsExecutionBuilderStage#executing(Consumer)} would
* exit, the command will be canceled by the scheduler.
*
* @param endCondition An optional end condition for the command. May be null.
* @return This builder object, for chaining.
*/
NeedsNameBuilderStage until(BooleanSupplier endCondition);
/**
* Creates the command based on the options set during previous builder stages. The builders will
* no longer be usable after calling this method.
*
* @param name The name of the command
* @return The built command.
* @throws NullPointerException If {@code name} is null.
*/
Command named(String name);
}

107
commandsv3/src/main/java/org/wpilib/commands3/ParallelGroup.java Normal file
View File

@@ -0,0 +1,107 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.Collection;
import java.util.HashSet;
import java.util.LinkedHashSet;
import java.util.Set;
/**
* A type of command that runs multiple other commands in parallel. The group will end after all
* required commands have completed; if no command is explicitly required for completion, then the
* group will end after any command in the group finishes. Any commands still running when the group
* has reached its completion condition will be canceled.
*/
public final class ParallelGroup implements Command {
private final Collection<Command> m_requiredCommands = new HashSet<>();
private final Collection<Command> m_optionalCommands = new LinkedHashSet<>();
private final Set<Mechanism> m_requirements = new HashSet<>();
private final String m_name;
private final int m_priority;
/**
* Creates a new parallel group.
*
* @param name The name of the group.
* @param requiredCommands The commands that are required to complete for the group to finish. If
* this is empty, then the group will finish when <i>any</i> optional command completes.
* @param optionalCommands The commands that do not need to complete for the group to finish. If
* this is empty, then the group will finish when <i>all</i> required commands complete.
*/
ParallelGroup(
String name, Collection<Command> requiredCommands, Collection<Command> optionalCommands) {
requireNonNullParam(name, "name", "ParallelGroup");
requireNonNullParam(requiredCommands, "requiredCommands", "ParallelGroup");
requireNonNullParam(optionalCommands, "optionalCommands", "ParallelGroup");
int i = 0;
for (Command requiredCommand : requiredCommands) {
requireNonNullParam(requiredCommand, "requiredCommands[" + i + "]", "ParallelGroup");
i++;
}
i = 0;
for (Command c : optionalCommands) {
requireNonNullParam(c, "optionalCommands[" + i + "]", "ParallelGroup");
i++;
}
var allCommands = new LinkedHashSet<Command>();
allCommands.addAll(requiredCommands);
allCommands.addAll(optionalCommands);
ConflictDetector.throwIfConflicts(allCommands);
m_name = name;
m_optionalCommands.addAll(optionalCommands);
m_requiredCommands.addAll(requiredCommands);
for (var command : allCommands) {
m_requirements.addAll(command.requirements());
}
m_priority =
allCommands.stream().mapToInt(Command::priority).max().orElse(Command.DEFAULT_PRIORITY);
}
@Override
public void run(Coroutine coroutine) {
coroutine.fork(m_optionalCommands);
if (m_requiredCommands.isEmpty()) {
// No required commands - just wait for the first optional command to finish
coroutine.awaitAny(m_optionalCommands);
} else {
// Wait for every required command to finish
coroutine.awaitAll(m_requiredCommands);
}
// The scheduler will cancel any optional child commands that are still running when
// the `run` method returns
}
@Override
public String name() {
return m_name;
}
@Override
public Set<Mechanism> requirements() {
return m_requirements;
}
@Override
public int priority() {
return m_priority;
}
@Override
public String toString() {
return "ParallelGroup[name=" + m_name + "]";
}
}

147
commandsv3/src/main/java/org/wpilib/commands3/ParallelGroupBuilder.java Normal file
View File

@@ -0,0 +1,147 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.Arrays;
import java.util.LinkedHashSet;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;
/**
* A builder class to configure and then create a {@link ParallelGroup}. Like {@link
* StagedCommandBuilder}, the final command is created by calling the terminal {@link
* #named(String)} method, or with an automatically generated name using {@link
* #withAutomaticName()}.
*/
@NoDiscard
public class ParallelGroupBuilder {
private final Set<Command> m_commands = new LinkedHashSet<>();
private final Set<Command> m_requiredCommands = new LinkedHashSet<>();
private BooleanSupplier m_endCondition;
/**
* Creates a new parallel group builder. The builder will have no commands and have no preapplied
* configuration options.
*/
public ParallelGroupBuilder() {}
/**
* Adds one or more optional commands to the group. They will not be required to complete for the
* parallel group to exit, and will be canceled once all required commands have finished.
*
* @param commands The optional commands to add to the group
* @return The builder object, for chaining
*/
public ParallelGroupBuilder optional(Command... commands) {
requireNonNullParam(commands, "commands", "ParallelGroupBuilder.optional");
for (int i = 0; i < commands.length; i++) {
requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.optional");
}
m_commands.addAll(Arrays.asList(commands));
return this;
}
/**
* Adds one or more required commands to the group. All required commands will need to complete
* for the group to exit.
*
* @param commands The required commands to add to the group
* @return The builder object, for chaining
*/
public ParallelGroupBuilder requiring(Command... commands) {
requireNonNullParam(commands, "commands", "ParallelGroupBuilder.requiring");
for (int i = 0; i < commands.length; i++) {
requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.requiring");
}
m_requiredCommands.addAll(Arrays.asList(commands));
m_commands.addAll(m_requiredCommands);
return this;
}
/**
* Adds a command to the group. The command must complete for the group to exit.
*
* @param command The command to add to the group
* @return The builder object, for chaining
*/
// Note: this primarily exists so users can cleanly chain .alongWith calls to build a
// parallel group, eg `fooCommand().alongWith(barCommand()).alongWith(bazCommand())`
public ParallelGroupBuilder alongWith(Command command) {
return requiring(command);
}
/**
* Adds an end condition to the command group. If this condition is met before all required
* commands have completed, the group will exit early. If multiple end conditions are added (e.g.
* {@code .until(() -> conditionA()).until(() -> conditionB())}), then the last end condition
* added will be used and any previously configured condition will be overridden.
*
* @param condition The end condition for the group. May be null.
* @return The builder object, for chaining
*/
public ParallelGroupBuilder until(BooleanSupplier condition) {
m_endCondition = condition;
return this;
}
/**
* Creates the group, using the provided named. The group will require everything that the
* commands in the group require, and will have a priority level equal to the highest priority
* among those commands.
*
* @param name The name of the parallel group
* @return The built group
*/
public ParallelGroup named(String name) {
requireNonNullParam(name, "name", "ParallelGroupBuilder.named");
var group = new ParallelGroup(name, m_commands, m_requiredCommands);
if (m_endCondition == null) {
// No custom end condition, return the group as is
return group;
}
// We have a custom end condition, so we need to wrap the group in a race
return new ParallelGroupBuilder()
.optional(group, Command.waitUntil(m_endCondition).named("Until Condition"))
.named(name);
}
/**
* Creates the group, giving it a name based on the commands within the group.
*
* @return The built group
*/
public ParallelGroup withAutomaticName() {
// eg "(CommandA & CommandB & CommandC)"
String required =
m_requiredCommands.stream().map(Command::name).collect(Collectors.joining(" & ", "(", ")"));
var optionalCommands = new LinkedHashSet<>(m_commands);
optionalCommands.removeAll(m_requiredCommands);
// eg "(CommandA | CommandB | CommandC)"
String optional =
optionalCommands.stream().map(Command::name).collect(Collectors.joining(" | ", "(", ")"));
if (m_requiredCommands.isEmpty()) {
// No required commands, pure race
return named(optional);
} else if (optionalCommands.isEmpty()) {
// Everything required
return named(required);
} else {
// Some form of deadline
// eg "[(CommandA & CommandB) * (CommandX | CommandY | ...)]"
String name = "[%s * %s]".formatted(required, optional);
return named(name);
}
}
}

970
commandsv3/src/main/java/org/wpilib/commands3/Scheduler.java Normal file
View File

@@ -0,0 +1,970 @@
// 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.commands3;
import static edu.wpi.first.units.Units.Microseconds;
import static edu.wpi.first.units.Units.Milliseconds;
import edu.wpi.first.util.ErrorMessages;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.event.EventLoop;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.HashSet;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.SequencedMap;
import java.util.SequencedSet;
import java.util.Set;
import java.util.Stack;
import java.util.function.Consumer;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;
import org.wpilib.commands3.button.CommandGenericHID;
import org.wpilib.commands3.proto.SchedulerProto;
/**
* Manages the lifecycles of {@link Coroutine}-based {@link Command Commands}. Commands may be
* scheduled directly using {@link #schedule(Command)}, or be bound to {@link Trigger Triggers} to
* automatically handle scheduling and cancellation based on internal or external events. User code
* is responsible for calling {@link #run()} periodically to update trigger conditions and execute
* scheduled commands. Most often, this is done by overriding {@link TimedRobot#robotPeriodic()} to
* include a call to {@code Scheduler.getDefault().run()}:
*
* <pre>{@code
* public class Robot extends TimedRobot {
* @Override
* public void robotPeriodic() {
* // Update the scheduler on every loop
* Scheduler.getDefault().run();
* }
* }
* }</pre>
*
* <h2>Danger</h2>
*
* <p>The scheduler <i>must</i> be used in a single-threaded program. Commands must be scheduled and
* canceled by the same thread that runs the scheduler, and cannot be run in a virtual thread.
*
* <p><strong>Using the commands framework in a multithreaded environment can cause crashes in the
* Java virtual machine at any time, including on an official field during a match.</strong> The
* Java JIT compilers make assumptions that rely on coroutines being used on a single thread.
* Breaking those assumptions can cause incorrect JIT code to be generated with undefined behavior,
* potentially causing control issues or crashes deep in JIT-generated native code.
*
* <p><strong>Normal concurrency constructs like locks, atomic references, and synchronized blocks
* or methods cannot save you.</strong>
*
* <h2>Lifecycle</h2>
*
* <p>The {@link #run()} method runs five steps:
*
* <ol>
* <li>Call {@link #sideload(Consumer) periodic sideload functions}
* <li>Poll all registered triggers to queue and cancel commands
* <li>Queue default commands for any mechanisms without a running command. The queued commands
* can be superseded by any manual scheduling or commands scheduled by triggers in the next
* run.
* <li>Start all queued commands. This happens after all triggers are checked in case multiple
* commands with conflicting requirements are queued in the same update; the last command to
* be queued takes precedence over the rest.
* <li>Loop over all running commands, mounting and calling each in turn until they either exit or
* call {@link Coroutine#yield()}. Commands run in the order in which they were scheduled.
* </ol>
*
* <h2>Telemetry</h2>
*
* <p>There are two mechanisms for telemetry for a scheduler. A protobuf serializer can be used to
* take a snapshot of a scheduler instance, and report what commands are queued (scheduled but have
* not yet started to run), commands that are running (along with timing data for each command), and
* total time spent in the most recent {@link #run()} call. However, it cannot detect one-shot
* commands that are scheduled, run, and complete all in a single {@code run()} invocation -
* effectively, commands that never call {@link Coroutine#yield()} are invisible.
*
* <p>A second telemetry mechanism is provided by {@link #addEventListener(Consumer)}. The scheduler
* will issue events to all registered listeners when certain events occur (see {@link
* SchedulerEvent} for all event types). These events are emitted immediately and can be used to
* detect lifecycle events for all commands, including one-shots that would be invisible to the
* 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 set of commands scheduled since the start of the previous run. */
private final SequencedSet<CommandState> m_queuedToRun = new LinkedHashSet<>();
/**
* The states of all running commands (does not include on deck commands). We preserve insertion
* order to guarantee that child commands run after their parents.
*/
private final SequencedMap<Command, CommandState> m_runningCommands = new LinkedHashMap<>();
/**
* The stack of currently executing commands. Child commands are pushed onto the stack and popped
* when they complete. Use {@link #currentState()} and {@link #currentCommand()} to get the
* currently executing command or its state.
*/
private final Stack<CommandState> m_currentCommandAncestry = new Stack<>();
/** The periodic callbacks to run, outside of the command structure. */
private final List<Coroutine> m_periodicCallbacks = new ArrayList<>();
/** Event loop for trigger bindings. */
private final EventLoop m_eventLoop = new EventLoop();
/** The scope for continuations to yield to. */
private final ContinuationScope m_scope = new ContinuationScope("coroutine commands");
// Telemetry
/** Protobuf serializer for a scheduler. */
public static final SchedulerProto proto = new SchedulerProto();
private double m_lastRunTimeMs = -1;
private final Set<Consumer<? super SchedulerEvent>> m_eventListeners = new LinkedHashSet<>();
/** The default scheduler instance. */
private static final Scheduler s_defaultScheduler = new Scheduler();
/**
* Gets the default scheduler instance for use in a robot program. Unless otherwise specified,
* triggers and mechanisms will be registered with the default scheduler and require the default
* scheduler to run. However, triggers and mechanisms can be constructed to be registered with a
* specific scheduler instance, which may be useful for isolation for unit tests.
*
* @return the default scheduler instance.
*/
@NoDiscard
public static Scheduler getDefault() {
return s_defaultScheduler;
}
/**
* Creates a new scheduler object. Note that most built-in constructs like {@link Trigger} and
* {@link CommandGenericHID} will use the {@link #getDefault() default scheduler instance} unless
* they were explicitly constructed with a different scheduler instance. Teams should use the
* default instance for convenience; however, new scheduler instances can be useful for unit
* tests.
*
* @return a new scheduler instance that is independent of the default scheduler instance.
*/
@NoDiscard
public static Scheduler createIndependentScheduler() {
return new Scheduler();
}
/** Private constructor. Use static factory methods or the default scheduler instance. */
private Scheduler() {}
/**
* Sets the default command for a mechanism. The command must require that mechanism, and cannot
* require any other mechanisms.
*
* @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
*/
public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) {
if (!defaultCommand.requires(mechanism)) {
throw new IllegalArgumentException(
"A mechanism's default command must require that mechanism");
}
if (defaultCommand.requirements().size() > 1) {
throw new IllegalArgumentException(
"A mechanism's default command cannot require other mechanisms");
}
m_defaultCommands.put(mechanism, defaultCommand);
}
/**
* Gets the default command set 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);
}
/**
* Adds a callback to run as part of the scheduler. The callback should not manipulate or control
* any mechanisms, but can be used to log information, update data (such as simulations or LED
* data buffers), or perform some other helpful task. The callback is responsible for managing its
* own control flow and end conditions. If you want to run a single task periodically for the
* entire lifespan of the scheduler, use {@link #addPeriodic(Runnable)}.
*
* <p><strong>Note:</strong> Like commands, any loops in the callback must appropriately yield
* control back to the scheduler with {@link Coroutine#yield} or risk stalling your program in an
* unrecoverable infinite loop!
*
* @param callback the callback to sideload
*/
public void sideload(Consumer<Coroutine> callback) {
var coroutine = new Coroutine(this, m_scope, callback);
m_periodicCallbacks.add(coroutine);
}
/**
* Adds a task to run repeatedly for as long as the scheduler runs. This internally handles the
* looping and control yielding necessary for proper function. The callback will run at the same
* periodic frequency as the scheduler.
*
* <p>For example:
*
* <pre>{@code
* scheduler.addPeriodic(() -> leds.setData(ledDataBuffer));
* scheduler.addPeriodic(() -> {
* SmartDashboard.putNumber("X", getX());
* SmartDashboard.putNumber("Y", getY());
* });
* }</pre>
*
* @param callback the periodic function to run
*/
public void addPeriodic(Runnable callback) {
sideload(
coroutine -> {
while (true) {
callback.run();
coroutine.yield();
}
});
}
/** Represents possible results of a command scheduling attempt. */
public enum ScheduleResult {
/** The command was successfully scheduled and added to the queue. */
SUCCESS,
/** The command is already scheduled or running. */
ALREADY_RUNNING,
/** The command is a lower priority and conflicts with a command that's already running. */
LOWER_PRIORITY_THAN_RUNNING_COMMAND,
}
/**
* Schedules a command to run. If one command schedules another (a "parent" and "child"), the
* child command will be canceled when the parent command completes. It is not possible to fork a
* child command and have it live longer than its parent.
*
* <p>Does nothing if the command is already scheduled or running, or requires at least one
* mechanism already used by a higher priority command.
*
* @param command the command to schedule
* @return the result of the scheduling attempt. See {@link ScheduleResult} for details.
* @throws IllegalArgumentException if scheduled by a command composition that has already
* scheduled another command that shares at least one required mechanism
*/
// Implementation detail: a child command will immediately start running when scheduled by a
// parent command, skipping the queue entirely. This avoids dead loop cycles where a parent
// schedules a child, appending it to the queue, then waits for the next cycle to pick it up and
// start it. With deeply nested commands, dead loops could quickly to add up and cause the
// innermost commands that actually _do_ something to start running hundreds of milliseconds after
// their root ancestor was scheduled.
public ScheduleResult schedule(Command command) {
// Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
// stack frame filtering and modification.
var binding =
new Binding(
BindingScope.global(), BindingType.IMMEDIATE, command, new Throwable().getStackTrace());
return schedule(binding);
}
// Package-private for use by Trigger
ScheduleResult schedule(Binding binding) {
var command = binding.command();
if (isScheduledOrRunning(command)) {
return ScheduleResult.ALREADY_RUNNING;
}
if (lowerPriorityThanConflictingCommands(command)) {
return ScheduleResult.LOWER_PRIORITY_THAN_RUNNING_COMMAND;
}
for (var scheduledState : m_queuedToRun) {
if (!command.conflictsWith(scheduledState.command())) {
// No shared requirements, skip
continue;
}
if (command.isLowerPriorityThan(scheduledState.command())) {
// Lower priority than an already-scheduled (but not yet running) command that requires at
// one of the same mechanism. Ignore it.
return ScheduleResult.LOWER_PRIORITY_THAN_RUNNING_COMMAND;
}
}
// 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
evictConflictingOnDeckCommands(command);
// If the binding is scoped to a particular command, that command is the parent. If we're in the
// middle of a run cycle and running commands, the parent is whatever command is currently
// running. Otherwise, there is no parent command.
var parentCommand =
binding.scope() instanceof BindingScope.ForCommand scope
? scope.command()
: currentCommand();
var state = new CommandState(command, parentCommand, buildCoroutine(command), binding);
emitScheduledEvent(command);
if (currentState() != null) {
// Scheduling a child command while running. Start it immediately instead of waiting a full
// cycle. This prevents issues with deeply nested command groups taking many scheduler cycles
// to start running the commands that actually _do_ things
evictConflictingRunningCommands(state);
m_runningCommands.put(command, state);
runCommand(state);
} else {
// Scheduling outside a command, add it to the pending set. If it's not overridden by another
// conflicting command being scheduled in the same scheduler loop, it'll be promoted and
// start to run when #runCommands() is called
m_queuedToRun.add(state);
}
return ScheduleResult.SUCCESS;
}
/**
* Checks if a command conflicts with and is a lower priority than any running command. Used when
* determining if the command can be scheduled.
*/
private boolean lowerPriorityThanConflictingCommands(Command command) {
Set<CommandState> ancestors = new HashSet<>();
for (var state = currentState(); state != null; state = m_runningCommands.get(state.parent())) {
ancestors.add(state);
}
// Check for conflicts with the commands that are already running
for (var state : m_runningCommands.values()) {
if (ancestors.contains(state)) {
// Can't conflict with an ancestor command
continue;
}
var c = state.command();
if (c.conflictsWith(command) && command.isLowerPriorityThan(c)) {
return true;
}
}
return false;
}
private void evictConflictingOnDeckCommands(Command command) {
for (var iterator = m_queuedToRun.iterator(); iterator.hasNext(); ) {
var scheduledState = iterator.next();
var scheduledCommand = scheduledState.command();
if (scheduledCommand.conflictsWith(command)) {
// Remove the lower priority conflicting command from the on deck commands.
// We don't need to call removeOrphanedChildren here because it hasn't started yet,
// meaning it hasn't had a chance to schedule any children
iterator.remove();
emitInterruptedEvent(scheduledCommand, command);
emitCanceledEvent(scheduledCommand);
}
}
}
/**
* Cancels all running commands with which an incoming state conflicts. Ancestor commands of the
* incoming state will not be canceled.
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
private void evictConflictingRunningCommands(CommandState incomingState) {
// The set of root states with which the incoming state conflicts but does not inherit from
Set<CommandState> conflictingRootStates =
m_runningCommands.values().stream()
.filter(state -> incomingState.command().conflictsWith(state.command()))
.filter(state -> !isAncestorOf(state.command(), incomingState))
.map(
state -> {
// Find the highest level ancestor of the conflicting command from which the
// incoming state does _not_ inherit. If they're totally unrelated, this will
// get the very root ancestor; otherwise, it'll return a direct sibling of the
// incoming command
CommandState root = state;
while (root.parent() != null && root.parent() != incomingState.parent()) {
root = m_runningCommands.get(root.parent());
}
return root;
})
.collect(Collectors.toSet());
// Cancel the root commands
for (var conflictingState : conflictingRootStates) {
emitInterruptedEvent(conflictingState.command(), incomingState.command());
cancel(conflictingState.command());
}
}
/**
* Checks if a particular command is an ancestor of another.
*
* @param ancestor the potential ancestor for which to search
* @param state the state to check
* @return true if {@code ancestor} is the direct parent or indirect ancestor, false if not
*/
@SuppressWarnings({"PMD.CompareObjectsWithEquals", "PMD.SimplifyBooleanReturns"})
private boolean isAncestorOf(Command ancestor, CommandState state) {
if (state.parent() == null) {
// No parent, cannot inherit
return false;
}
if (!m_runningCommands.containsKey(ancestor)) {
// The given ancestor isn't running
return false;
}
if (state.parent() == ancestor) {
// Direct child
return true;
}
// Check if the command's parent inherits from the given ancestor
return m_runningCommands.values().stream()
.filter(s -> state.parent() == s.command())
.anyMatch(s -> isAncestorOf(ancestor, s));
}
/**
* Cancels a command and any other command scheduled by it. This occurs immediately and does not
* need to wait for a call to {@link #run()}. Any command that it scheduled will also be canceled
* to ensure commands within compositions do not continue to run.
*
* <p>This has no effect if the given command is not currently scheduled or running.
*
* @param command the command to cancel
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public void cancel(Command command) {
if (command == currentCommand()) {
throw new IllegalArgumentException(
"Command `" + command.name() + "` is mounted and cannot be canceled");
}
boolean running = isRunning(command);
// Evict the command. The next call to run() will schedule the default command for all its
// required mechanisms, unless another command requiring those mechanisms is scheduled between
// calling cancel() and calling run()
m_runningCommands.remove(command);
m_queuedToRun.removeIf(state -> state.command() == command);
if (running) {
// Only run the hook if the command was running. If it was on deck or not
// even in the scheduler at the time, then there's nothing to do
command.onCancel();
emitCanceledEvent(command);
}
// Clean up any orphaned child commands; their lifespan must not exceed the parent's
removeOrphanedChildren(command);
}
/**
* Updates the command scheduler. This will run operations in the following order:
*
* <ol>
* <li>Run sideloaded functions from {@link #sideload(Consumer)} and {@link
* #addPeriodic(Runnable)}
* <li>Update trigger bindings to queue and cancel bound commands
* <li>Queue default commands for mechanisms that do not have a queued or running command
* <li>Promote queued commands to the running set
* <li>For every command in the running set, mount and run that command until it calls {@link
* Coroutine#yield()} or exits
* </ol>
*
* <p>This method is intended to be called in a periodic loop like {@link
* TimedRobot#robotPeriodic()}
*/
public void run() {
final long startMicros = RobotController.getTime();
// Sideloads may change some state that affects triggers. Run them first.
runPeriodicSideloads();
// Poll triggers next to schedule and cancel commands
m_eventLoop.poll();
// Schedule default commands for any mechanisms that don't have a running command and didn't
// have a new command scheduled by a sideload function or a trigger
scheduleDefaultCommands();
// Move all scheduled commands to the running set
promoteScheduledCommands();
// Run every command in order until they call Coroutine.yield() or exit
runCommands();
final long endMicros = RobotController.getTime();
m_lastRunTimeMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds);
}
private void promoteScheduledCommands() {
// Clear any commands that conflict with the scheduled set
for (var queuedState : m_queuedToRun) {
evictConflictingRunningCommands(queuedState);
}
// Move any scheduled commands to the running set
for (var queuedState : m_queuedToRun) {
m_runningCommands.put(queuedState.command(), queuedState);
}
// Clear the set of on-deck commands,
// since we just put them all into the set of running commands
m_queuedToRun.clear();
}
private void runPeriodicSideloads() {
// Update periodic callbacks
for (Coroutine coroutine : m_periodicCallbacks) {
coroutine.mount();
try {
coroutine.runToYieldPoint();
} finally {
Continuation.mountContinuation(null);
}
}
// And remove any periodic callbacks that have completed
m_periodicCallbacks.removeIf(Coroutine::isDone);
}
private void runCommands() {
// Tick every command that hasn't been completed yet
// Run in reverse so parent commands can resume in the same loop cycle an awaited child command
// completes. Otherwise, parents could only resume on the next loop cycle, introducing a delay
// at every layer of nesting.
for (var state : List.copyOf(m_runningCommands.values()).reversed()) {
runCommand(state);
}
}
/**
* Mounts and runs a command until it yields or exits.
*
* @param state The command state to run
*/
@SuppressWarnings("PMD.AvoidCatchingGenericException")
private void runCommand(CommandState state) {
final var command = state.command();
final var coroutine = state.coroutine();
if (!m_runningCommands.containsKey(command)) {
// Probably canceled by an owning composition, do not run
return;
}
var previousState = currentState();
m_currentCommandAncestry.push(state);
long startMicros = RobotController.getTime();
emitMountedEvent(command);
coroutine.mount();
try {
coroutine.runToYieldPoint();
} catch (RuntimeException e) {
// Command encountered an uncaught exception.
handleCommandException(state, e);
} finally {
long endMicros = RobotController.getTime();
double elapsedMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds);
state.setLastRuntimeMs(elapsedMs);
if (state.equals(currentState())) {
// Remove the command we just ran from the top of the stack
m_currentCommandAncestry.pop();
}
if (previousState != null) {
// Remount the parent command, if there is one
previousState.coroutine().mount();
} else {
Continuation.mountContinuation(null);
}
}
if (coroutine.isDone()) {
// Immediately check if the command has completed and remove any children commands.
// This prevents child commands from being executed one extra time in the run() loop
emitCompletedEvent(command);
m_runningCommands.remove(command);
removeOrphanedChildren(command);
} else {
// Yielded
emitYieldedEvent(command);
}
}
/**
* Handles uncaught runtime exceptions from a mounted and running command. The command's ancestor
* and child commands will all be canceled and the exception's backtrace will be modified to
* include the stack frames of the schedule call site.
*
* @param state The state of the command that encountered the exception.
* @param e The exception that was thrown.
* @throws RuntimeException rethrows the exception, with a modified backtrace pointing to the
* schedule site
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
private void handleCommandException(CommandState state, RuntimeException e) {
var command = state.command();
// Fetch the root command
// (needs to be done before removing the failed command from the running set)
Command root = command;
while (getParentOf(root) != null) {
root = getParentOf(root);
}
// Remove it from the running set.
m_runningCommands.remove(command);
// Intercept the exception, inject stack frames from the schedule site, and rethrow it
var binding = state.binding();
e.setStackTrace(CommandTraceHelper.modifyTrace(e.getStackTrace(), binding.frames()));
emitCompletedWithErrorEvent(command, e);
// Clean up child commands after emitting the event so child Canceled events are emitted
// after the parent's CompletedWithError
removeOrphanedChildren(command);
// Bubble up to the root and cancel all commands between the root and this one
// Note: Because we remove the command from the running set above, we still need to
// clean up all the failed command's children
if (root != null && root != command) {
cancel(root);
}
// Then rethrow the exception
throw e;
}
/**
* Gets the currently executing command state, or null if no command is currently executing.
*
* @return the currently executing command state
*/
private CommandState currentState() {
if (m_currentCommandAncestry.isEmpty()) {
// Avoid EmptyStackException
return null;
}
return m_currentCommandAncestry.peek();
}
/**
* Gets the currently executing command, or null if no command is currently executing.
*
* @return the currently executing command
*/
public Command currentCommand() {
var state = currentState();
if (state == null) {
return null;
}
return state.command();
}
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);
}
});
}
/**
* Removes all commands descended from a parent command. This is used to ensure that any command
* scheduled within a composition or group cannot live longer than any ancestor.
*
* @param parent the root command whose descendants to remove from the scheduler
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
private void removeOrphanedChildren(Command parent) {
m_runningCommands.entrySet().stream()
.filter(e -> e.getValue().parent() == parent)
.toList() // copy to an intermediate list to avoid concurrent modification
.forEach(e -> cancel(e.getKey()));
}
/**
* Builds a coroutine object that the command will be bound to. The coroutine will be scoped to
* this scheduler object and cannot be used by another scheduler instance.
*
* @param command the command for which to build a coroutine
* @return the binding coroutine
*/
private Coroutine buildCoroutine(Command command) {
return new Coroutine(this, m_scope, command::run);
}
/**
* Checks if a command is currently running.
*
* @param command the command to check
* @return true if the command is running, false if not
*/
public boolean isRunning(Command command) {
return m_runningCommands.containsKey(command);
}
/**
* Checks if a command is currently scheduled to run, but is not yet running.
*
* @param command the command to check
* @return true if the command is scheduled to run, false if not
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public boolean isScheduled(Command command) {
return m_queuedToRun.stream().anyMatch(state -> state.command() == command);
}
/**
* Checks if a command is currently scheduled to run, or is already running.
*
* @param command the command to check
* @return true if the command is scheduled to run or is already running, false if not
*/
public boolean isScheduledOrRunning(Command command) {
return isScheduled(command) || isRunning(command);
}
/**
* Gets the set of all currently running commands. Commands are returned in the order in which
* they were scheduled. The returned set is read-only.
*
* @return the currently running commands
*/
public Collection<Command> getRunningCommands() {
return Collections.unmodifiableSet(m_runningCommands.keySet());
}
/**
* Gets all the currently running commands that require a particular mechanism. Commands are
* returned in the order in which they were scheduled. The returned list is read-only.
*
* @param mechanism the mechanism to get the commands for
* @return the currently running commands that require the mechanism.
*/
public List<Command> getRunningCommandsFor(Mechanism mechanism) {
return m_runningCommands.keySet().stream()
.filter(command -> command.requires(mechanism))
.toList();
}
/**
* Cancels all currently running and scheduled commands. All default commands will be scheduled on
* the next call to {@link #run()}, unless a higher priority command is scheduled or triggered
* after {@code cancelAll()} is used.
*/
public void cancelAll() {
for (var onDeckIter = m_queuedToRun.iterator(); onDeckIter.hasNext(); ) {
var state = onDeckIter.next();
onDeckIter.remove();
emitCanceledEvent(state.command());
}
for (var liveIter = m_runningCommands.entrySet().iterator(); liveIter.hasNext(); ) {
var entry = liveIter.next();
liveIter.remove();
Command canceledCommand = entry.getKey();
canceledCommand.onCancel();
emitCanceledEvent(canceledCommand);
}
}
/**
* An event loop used by the scheduler to poll triggers that schedule or cancel commands. This
* event loop is always polled on every call to {@link #run()}. Custom event loops need to be
* bound to this one for synchronicity with the scheduler; however, they can always be polled
* manually before or after the call to {@link #run()}.
*
* @return the default event loop.
*/
@NoDiscard
public EventLoop getDefaultEventLoop() {
return m_eventLoop;
}
/**
* For internal use.
*
* @return The commands that have been scheduled but not yet started.
*/
@NoDiscard
public Collection<Command> getQueuedCommands() {
return m_queuedToRun.stream().map(CommandState::command).toList();
}
/**
* For internal use.
*
* @param command The command to check
* @return The command that forked the provided command. Null if the command is not a child of
* another command.
*/
public Command getParentOf(Command command) {
var state = m_runningCommands.get(command);
if (state == null) {
return null;
}
return state.parent();
}
/**
* Gets how long a command took to run in the previous cycle. If the command is not currently
* running, returns -1.
*
* @param command The command to check
* @return How long, in milliseconds, the command last took to execute.
*/
public double lastCommandRuntimeMs(Command command) {
if (m_runningCommands.containsKey(command)) {
return m_runningCommands.get(command).lastRuntimeMs();
} else {
return -1;
}
}
/**
* Gets how long a command has taken to run, in aggregate, since it was most recently scheduled.
* If the command is not currently running, returns -1.
*
* @param command The command to check
* @return How long, in milliseconds, the command has taken to execute in total
*/
public double totalRuntimeMs(Command command) {
if (m_runningCommands.containsKey(command)) {
return m_runningCommands.get(command).totalRuntimeMs();
} else {
// Not running; no data
return -1;
}
}
/**
* Gets the unique run id for a scheduled or running command. If the command is not currently
* scheduled or running, this function returns {@code 0}.
*
* @param command The command to get the run ID for
* @return The run of the command
*/
@NoDiscard
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public int runId(Command command) {
if (m_runningCommands.containsKey(command)) {
return m_runningCommands.get(command).id();
}
// Check scheduled commands
for (var scheduled : m_queuedToRun) {
if (scheduled.command() == command) {
return scheduled.id();
}
}
return 0;
}
/**
* Gets how long the scheduler took to process its most recent {@link #run()} invocation, in
* milliseconds. Defaults to -1 if the scheduler has not yet run.
*
* @return How long, in milliseconds, the scheduler last took to execute.
*/
@NoDiscard
public double lastRuntimeMs() {
return m_lastRunTimeMs;
}
// Event-base telemetry and helpers. The static factories are for convenience to automatically
// set the timestamp instead of littering RobotController.getTime() everywhere.
private void emitScheduledEvent(Command command) {
var event = new SchedulerEvent.Scheduled(command, RobotController.getTime());
emitEvent(event);
}
private void emitMountedEvent(Command command) {
var event = new SchedulerEvent.Mounted(command, RobotController.getTime());
emitEvent(event);
}
private void emitYieldedEvent(Command command) {
var event = new SchedulerEvent.Yielded(command, RobotController.getTime());
emitEvent(event);
}
private void emitCompletedEvent(Command command) {
var event = new SchedulerEvent.Completed(command, RobotController.getTime());
emitEvent(event);
}
private void emitCompletedWithErrorEvent(Command command, Throwable error) {
var event = new SchedulerEvent.CompletedWithError(command, error, RobotController.getTime());
emitEvent(event);
}
private void emitCanceledEvent(Command command) {
var event = new SchedulerEvent.Canceled(command, RobotController.getTime());
emitEvent(event);
}
private void emitInterruptedEvent(Command command, Command interrupter) {
var event = new SchedulerEvent.Interrupted(command, interrupter, RobotController.getTime());
emitEvent(event);
}
/**
* Adds a listener to handle events that are emitted by the scheduler. Events are emitted when
* certain actions are taken by user code or by internal processing logic in the scheduler.
* Listeners should take care to be quick, simple, and not schedule or cancel commands, as that
* may cause inconsistent scheduler behavior or even cause a program crash.
*
* <p>Listeners are primarily expected to be for data logging and telemetry. In particular, a
* one-shot command (one that never calls {@link Coroutine#yield()}) will never appear in the
* standard protobuf telemetry because it is scheduled, runs, and finishes all in a single
* scheduler cycle. However, {@link SchedulerEvent.Scheduled},{@link SchedulerEvent.Mounted}, and
* {@link SchedulerEvent.Completed} events will be emitted corresponding to those actions, and
* user code can listen for and log such events.
*
* @param listener The listener to add. Cannot be null.
* @throws NullPointerException if given a null listener
*/
public void addEventListener(Consumer<? super SchedulerEvent> listener) {
ErrorMessages.requireNonNullParam(listener, "listener", "addEventListener");
m_eventListeners.add(listener);
}
private void emitEvent(SchedulerEvent event) {
// TODO: Prevent listeners from interacting with the scheduler.
// Scheduling or canceling commands while the scheduler is processing will probably cause
// bugs in user code or even a program crash.
for (var listener : m_eventListeners) {
listener.accept(event);
}
}
}

89
commandsv3/src/main/java/org/wpilib/commands3/SchedulerEvent.java Normal file
View File

@@ -0,0 +1,89 @@
// 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.commands3;
import edu.wpi.first.wpilibj.RobotController;
import java.util.function.Consumer;
/**
* An event that occurs during scheduler processing. This can range from {@link Scheduled a command
* being scheduled} by a trigger or manual call to {@link Scheduler#schedule(Command)} to {@link
* Interrupted a command being interrupted by another}. Event listeners can be registered with a
* scheduler via {@link Scheduler#addEventListener(Consumer)}. All events have a timestamp to
* indicate when the event occurred.
*/
public sealed interface SchedulerEvent {
/**
* The timestamp for when the event occurred. Measured in microseconds since some arbitrary start
* time.
*
* @return The event timestamp.
* @see RobotController#getTime()
*/
long timestampMicros();
/**
* An event marking when a command is scheduled in {@link Scheduler#schedule(Command)}.
*
* @param command The command that was scheduled
* @param timestampMicros When the command was scheduled
*/
record Scheduled(Command command, long timestampMicros) implements SchedulerEvent {}
/**
* An event marking when a command starts running.
*
* @param command The command that started
* @param timestampMicros When the command started
*/
record Mounted(Command command, long timestampMicros) implements SchedulerEvent {}
/**
* An event marking when a command yielded control with {@link Coroutine#yield()}.
*
* @param command The command that yielded
* @param timestampMicros When the command yielded
*/
record Yielded(Command command, long timestampMicros) implements SchedulerEvent {}
/**
* An event marking when a command completed naturally.
*
* @param command The command that completed
* @param timestampMicros When the command completed
*/
record Completed(Command command, long timestampMicros) implements SchedulerEvent {}
/**
* An event marking when a command threw or encountered an unhanded exception.
*
* @param command The command that encountered the error
* @param error The unhandled error
* @param timestampMicros When the error occurred
*/
record CompletedWithError(Command command, Throwable error, long timestampMicros)
implements SchedulerEvent {}
/**
* An event marking when a command was canceled. If the command was canceled because it was
* interrupted by another command, an {@link Interrupted} will be emitted immediately prior to the
* cancellation event.
*
* @param command The command that was canceled
* @param timestampMicros When the command was canceled
*/
record Canceled(Command command, long timestampMicros) implements SchedulerEvent {}
/**
* An event marking when a command was interrupted by another. Typically followed by an {@link
* Canceled} event.
*
* @param command The command that was interrupted
* @param interrupter The interrupting command
* @param timestampMicros When the command was interrupted
*/
record Interrupted(Command command, Command interrupter, long timestampMicros)
implements SchedulerEvent {}
}

80
commandsv3/src/main/java/org/wpilib/commands3/SequentialGroup.java Normal file
View File

@@ -0,0 +1,80 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
/**
* A sequence of commands that run one after another. Each successive command only starts after its
* predecessor completes execution. The priority of a sequence is equal to the highest priority of
* any of its commands. If all commands in the sequence are able to run while the robot is disabled,
* then the sequence itself will be allowed to run while the robot is disabled.
*
* <p>The sequence will <i>own</i> all mechanisms required by all commands in the sequence for the
* entire duration of the sequence. This means that a mechanism owned by one command in the
* sequence, but not by a later one, will be <i>uncommanded</i> while that later command executes.
*/
public final class SequentialGroup implements Command {
private final String m_name;
private final List<Command> m_commands = new ArrayList<>();
private final Set<Mechanism> m_requirements = new HashSet<>();
private final int m_priority;
/**
* Creates a new command sequence.
*
* @param name the name of the sequence
* @param commands the commands to execute within the sequence
*/
SequentialGroup(String name, List<Command> commands) {
requireNonNullParam(name, "name", "SequentialGroup");
requireNonNullParam(commands, "commands", "SequentialGroup");
for (int i = 0; i < commands.size(); i++) {
requireNonNullParam(commands.get(i), "commands[" + i + "]", "SequentialGroup");
}
m_name = name;
m_commands.addAll(commands);
for (var command : commands) {
m_requirements.addAll(command.requirements());
}
m_priority =
commands.stream().mapToInt(Command::priority).max().orElse(Command.DEFAULT_PRIORITY);
}
@Override
public void run(Coroutine coroutine) {
for (var command : m_commands) {
coroutine.await(command);
}
}
@Override
public String name() {
return m_name;
}
@Override
public Set<Mechanism> requirements() {
return m_requirements;
}
@Override
public int priority() {
return m_priority;
}
@Override
public String toString() {
return "SequentialGroup[name=" + m_name + "]";
}
}

106
commandsv3/src/main/java/org/wpilib/commands3/SequentialGroupBuilder.java Normal file
View File

@@ -0,0 +1,106 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;
/**
* A builder class to configure and then create a {@link SequentialGroup}. Like {@link
* StagedCommandBuilder}, the final command is created by calling the terminal {@link
* #named(String)} method, or with an automatically generated name using {@link
* #withAutomaticName()}.
*/
@NoDiscard
public class SequentialGroupBuilder {
private final List<Command> m_steps = new ArrayList<>();
private BooleanSupplier m_endCondition;
/**
* Creates new SequentialGroupBuilder. The builder will have no commands and have no preapplied
* configuration options. Use {@link #andThen(Command)} to add commands to the sequence.
*/
public SequentialGroupBuilder() {}
/**
* Adds a command to the sequence.
*
* @param next The next command in the sequence
* @return The builder object, for chaining
*/
public SequentialGroupBuilder andThen(Command next) {
requireNonNullParam(next, "next", "SequentialGroupBuilder.andThen");
m_steps.add(next);
return this;
}
/**
* Adds commands to the sequence. Commands will be added to the sequence in the order they are
* passed to this method.
*
* @param nextCommands The next commands in the sequence
* @return The builder object, for chaining
*/
public SequentialGroupBuilder andThen(Command... nextCommands) {
requireNonNullParam(nextCommands, "nextCommands", "SequentialGroupBuilder.andThen");
for (int i = 0; i < nextCommands.length; i++) {
requireNonNullParam(
nextCommands[i], "nextCommands[" + i + "]", "SequentialGroupBuilder.andThen");
}
m_steps.addAll(Arrays.asList(nextCommands));
return this;
}
/**
* Adds an end condition to the command group. If this condition is met before all required
* commands have completed, the group will exit early. If multiple end conditions are added (e.g.
* {@code .until(() -> conditionA()).until(() -> conditionB())}), then the last end condition
* added will be used and any previously configured condition will be overridden.
*
* @param endCondition The end condition for the group
* @return The builder object, for chaining
*/
public SequentialGroupBuilder until(BooleanSupplier endCondition) {
m_endCondition = endCondition;
return this;
}
/**
* Creates the sequence command, giving it the specified name.
*
* @param name The name of the sequence command
* @return The built command
*/
public Command named(String name) {
var seq = new SequentialGroup(name, m_steps);
if (m_endCondition != null) {
// No custom end condition, return the group as is
return seq;
}
// We have a custom end condition, so we need to wrap the group in a race
return new ParallelGroupBuilder()
.optional(seq, Command.waitUntil(m_endCondition).named("Until Condition"))
.named(name);
}
/**
* Creates the sequence command, giving it an automatically generated name based on the commands
* in the sequence.
*
* @return The built command
*/
public Command withAutomaticName() {
return named(m_steps.stream().map(Command::name).collect(Collectors.joining(" -> ")));
}
}

299
commandsv3/src/main/java/org/wpilib/commands3/StagedCommandBuilder.java Normal file
View File

@@ -0,0 +1,299 @@
// 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.commands3;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import java.util.Arrays;
import java.util.Collection;
import java.util.HashSet;
import java.util.Objects;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import org.wpilib.annotation.NoDiscard;
/**
* A builder class for commands. Command configuration is done in stages, where later stages have
* different configuration options than earlier stages. Commands may only be created after going
* through every stage, enforcing the presence of required options. All commands <i>must</i> have a
* set of requirements (which may be empty), a name, and an implementation. The builder stages are
* defined such that the final stage that creates a command can only be reached after going through
* the earlier stages to configure those required options.
*
* <p>Example usage:
*
* <pre>{@code
* StagedCommandBuilder start = new StagedCommandBuilder();
* NeedsExecutionBuilderStage withRequirements = start.requiring(mechanism1, mechanism2);
* NeedsNameBuilderStage withExecution = withRequirements.executing(coroutine -> ...);
* Command exampleCommand = withExecution.named("Example Command");
* }</pre>
*
* <p>Because every method on the builders returns a builder object, these calls can be chained to
* cut down on verbosity and make the code easier to read. This is the recommended style:
*
* <pre>{@code
* Command exampleCommand =
* new StagedCommandBuilder()
* .requiring(mechanism1, mechanism2)
* .executing(coroutine -> ...)
* .named("Example Command");
* }</pre>
*
* <p>And can be cut down even further by using helper methods provided by the library:
*
* <pre>{@code
* Command exampleCommand =
* Command
* .requiring(mechanism1, mechanism2)
* .executing(coroutine -> ...)
* .named("Example Command");
* }</pre>
*/
@NoDiscard
public final class StagedCommandBuilder {
private final Set<Mechanism> m_requirements = new HashSet<>();
private Consumer<Coroutine> m_impl;
private Runnable m_onCancel = () -> {};
private String m_name;
private int m_priority = Command.DEFAULT_PRIORITY;
private BooleanSupplier m_endCondition;
private Command m_builtCommand;
// Using internal anonymous classes to implement the staged builder APIs, but all backed by the
// state of the enclosing StagedCommandBuilder object
private final NeedsExecutionBuilderStage m_needsExecutionView =
new NeedsExecutionBuilderStage() {
@Override
public NeedsExecutionBuilderStage requiring(Mechanism requirement) {
throwIfAlreadyBuilt();
requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring");
m_requirements.add(requirement);
return this;
}
@Override
public NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra) {
throwIfAlreadyBuilt();
requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring");
requireNonNullParam(extra, "extra", "StagedCommandBuilder.requiring");
for (int i = 0; i < extra.length; i++) {
requireNonNullParam(extra[i], "extra[" + i + "]", "StagedCommandBuilder.requiring");
}
m_requirements.add(requirement);
m_requirements.addAll(Arrays.asList(extra));
return this;
}
@Override
public NeedsExecutionBuilderStage requiring(Collection<Mechanism> requirements) {
throwIfAlreadyBuilt();
requireNonNullParam(requirements, "requirements", "StagedCommandBuilder.requiring");
int i = 0;
for (Mechanism requirement : requirements) {
requireNonNullParam(
requirement, "requirements[" + i + "]", "StagedCommandBuilder.requiring");
i++;
}
m_requirements.addAll(requirements);
return this;
}
@Override
public NeedsNameBuilderStage executing(Consumer<Coroutine> impl) {
throwIfAlreadyBuilt();
requireNonNullParam(impl, "impl", "StagedCommandBuilder.executing");
m_impl = impl;
return m_needsNameView;
}
};
private final NeedsNameBuilderStage m_needsNameView =
new NeedsNameBuilderStage() {
@Override
public NeedsNameBuilderStage whenCanceled(Runnable onCancel) {
throwIfAlreadyBuilt();
m_onCancel = onCancel;
return this;
}
@Override
public NeedsNameBuilderStage withPriority(int priority) {
throwIfAlreadyBuilt();
m_priority = priority;
return this;
}
@Override
public NeedsNameBuilderStage until(BooleanSupplier endCondition) {
throwIfAlreadyBuilt();
m_endCondition = endCondition; // allowed to be null
return this;
}
@Override
public Command named(String name) {
throwIfAlreadyBuilt();
requireNonNullParam(name, "name", "StagedCommandBuilder.withName");
m_name = name;
var command = new BuilderBackedCommand(StagedCommandBuilder.this);
if (m_endCondition == null) {
// No custom end condition, just return the raw command
m_builtCommand = command;
} else {
// A custom end condition is implemented as a race group, since we cannot modify the
// command body to inject the end condition.
m_builtCommand =
new ParallelGroupBuilder().requiring(command).until(m_endCondition).named(m_name);
}
return m_builtCommand;
}
};
private static final class BuilderBackedCommand implements Command {
private static final Runnable kNoOp = () -> {};
private final Set<Mechanism> m_requirements;
private final Consumer<Coroutine> m_impl;
private final Runnable m_onCancel;
private final String m_name;
private final int m_priority;
private BuilderBackedCommand(StagedCommandBuilder builder) {
// Copy builder fields into the command so the builder object can be garbage collected
m_requirements = new HashSet<>(builder.m_requirements);
m_impl = builder.m_impl;
m_onCancel = Objects.requireNonNullElse(builder.m_onCancel, kNoOp);
m_name = builder.m_name;
m_priority = builder.m_priority;
}
@Override
public void run(Coroutine coroutine) {
m_impl.accept(coroutine);
}
@Override
public void onCancel() {
m_onCancel.run();
}
@Override
public String name() {
return m_name;
}
@Override
public Set<Mechanism> requirements() {
return m_requirements;
}
@Override
public int priority() {
return m_priority;
}
@Override
public String toString() {
return name();
}
}
/**
* Creates a new command builder. All required options must be set on each stage before a command
* is able to be created. Attempting to create a command without setting all required options will
* result in a compilation error.
*/
public StagedCommandBuilder() {}
/**
* Explicitly marks the command as requiring no mechanisms. Unless overridden later with {@link
* NeedsExecutionBuilderStage#requiring(Mechanism)} or a similar method, the built command will
* not have ownership over any mechanisms when it runs. Use this for commands that don't need to
* own a mechanism, such as a gyro zeroing command, that does some kind of cleanup task without
* needing to control something.
*
* @return A builder object that can be used to further configure the command.
*/
public NeedsExecutionBuilderStage noRequirements() {
throwIfAlreadyBuilt();
return m_needsExecutionView;
}
/**
* Marks the command as requiring one or more mechanisms. If only a single mechanism is required,
* prefer a factory function like {@link Mechanism#run(Consumer)} or similar - it will
* automatically require the mechanism, instead of it needing to be explicitly specified.
*
* @param requirement The first required mechanism. Cannot be null.
* @param extra Any optional extra required mechanisms. May be empty, but cannot be null or
* contain null values.
* @return A builder object that can be used to further configure the command.
*/
public NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra) {
throwIfAlreadyBuilt();
requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring");
requireNonNullParam(extra, "extra", "StagedCommandBuilder.requiring");
for (int i = 0; i < extra.length; i++) {
requireNonNullParam(extra[i], "extra[" + i + "]", "StagedCommandBuilder.requiring");
}
m_requirements.add(requirement);
m_requirements.addAll(Arrays.asList(extra));
return m_needsExecutionView;
}
/**
* Marks the command as requiring zero or more mechanisms. If only a single mechanism is required,
* prefer a factory function like {@link Mechanism#run(Consumer)} or similar - it will
* automatically require the mechanism, instead of it needing to be explicitly specified.
*
* @param requirements A collection of required mechanisms. May be empty, but cannot be null or
* contain null values.
* @return A builder object that can be used to further configure the command.
*/
public NeedsExecutionBuilderStage requiring(Collection<Mechanism> requirements) {
throwIfAlreadyBuilt();
requireNonNullParam(requirements, "requirements", "StagedCommandBuilder.requiring");
int i = 0;
for (var mechanism : requirements) {
requireNonNullParam(mechanism, "requirements[" + i + "]", "StagedCommandBuilder.requiring");
i++;
}
m_requirements.addAll(requirements);
return m_needsExecutionView;
}
// Prevent builders from being mutated after command creation
// Weird things could happen like changing requirements, priority level, or even the command
// implementation itself if we didn't prohibit it.
private void throwIfAlreadyBuilt() {
if (m_builtCommand != null) {
throw new IllegalStateException("Command builders cannot be reused");
}
}
}

368
commandsv3/src/main/java/org/wpilib/commands3/Trigger.java Normal file
View File

@@ -0,0 +1,368 @@
// 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.commands3;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.wpilibj.event.EventLoop;
import java.util.ArrayList;
import java.util.EnumMap;
import java.util.List;
import java.util.Map;
import java.util.function.BooleanSupplier;
/**
* Triggers allow users to specify conditions for when commands should run. Triggers can be set up
* to read from joystick and controller buttons (eg {@link
* org.wpilib.commands3.button.CommandXboxController#x()}) or be customized to read sensor values or
* any other arbitrary true/false condition.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger button
* of a joystick to a "score" command.
*
* <p>Triggers can easily be composed for advanced functionality using the {@link
* #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, {@link #negate()} operators.
*
* <p>Trigger bindings created inside a running command will only be active while that command is
* running. This is useful for defining trigger-based behavior only in a certain scope and avoids
* needing to create dozens of global triggers. Any commands scheduled by these triggers will be
* canceled when the enclosing command exits.
*
* <pre>{@code
* Command shootWhileAiming = Command.noRequirements().executing(co -> {
* turret.atTarget.onTrue(shooter.shootOnce());
* co.await(turret.lockOnGoal());
* }).named("Shoot While Aiming");
* controller.rightBumper().whileTrue(shootWhileAiming);
* }</pre>
*/
public class Trigger implements BooleanSupplier {
private final BooleanSupplier m_condition;
private final EventLoop m_loop;
private final Scheduler m_scheduler;
private Signal m_previousSignal;
private final Map<BindingType, List<Binding>> m_bindings = new EnumMap<>(BindingType.class);
private final Runnable m_eventLoopCallback = this::poll;
private boolean m_isBoundToEventLoop; // used for lazily binding to the event loop
/**
* Represents the state of a signal: high or low. Used instead of a boolean for nullity on the
* first run, when the previous signal value is undefined and unknown.
*/
private enum Signal {
/** The signal is high. */
HIGH,
/** The signal is low. */
LOW
}
/**
* Creates a new trigger based on the given condition. Polled by the scheduler's {@link
* Scheduler#getDefaultEventLoop() default event loop}.
*
* @param scheduler The scheduler that should execute triggered commands.
* @param condition the condition represented by this trigger
*/
public Trigger(Scheduler scheduler, BooleanSupplier condition) {
m_scheduler = requireNonNullParam(scheduler, "scheduler", "Trigger");
m_loop = scheduler.getDefaultEventLoop();
m_condition = requireNonNullParam(condition, "condition", "Trigger");
}
/**
* Creates a new trigger based on the given condition. Triggered commands are executed by the
* {@link Scheduler#getDefault() default scheduler}.
*
* <p>Polled by the default scheduler button loop.
*
* @param condition the condition represented by this trigger
*/
public Trigger(BooleanSupplier condition) {
this(Scheduler.getDefault(), condition);
}
/**
* Creates a new trigger based on the given condition.
*
* @param scheduler The scheduler that should execute triggered commands.
* @param loop The event loop to poll the trigger.
* @param condition the condition represented by this trigger
*/
public Trigger(Scheduler scheduler, EventLoop loop, BooleanSupplier condition) {
m_scheduler = requireNonNullParam(scheduler, "scheduler", "Trigger");
m_loop = requireNonNullParam(loop, "loop", "Trigger");
m_condition = requireNonNullParam(condition, "condition", "Trigger");
}
/**
* Starts the given command whenever the condition changes from `false` to `true`.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger onTrue(Command command) {
requireNonNullParam(command, "command", "onTrue");
addBinding(BindingType.SCHEDULE_ON_RISING_EDGE, command);
return this;
}
/**
* Starts the given command whenever the condition changes from `true` to `false`.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger onFalse(Command command) {
requireNonNullParam(command, "command", "onFalse");
addBinding(BindingType.SCHEDULE_ON_FALLING_EDGE, command);
return this;
}
/**
* Starts the given command when the condition changes to `true` and cancels it when the condition
* changes to `false`.
*
* <p>Doesn't re-start the command if it ends while the condition is still `true`.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileTrue(Command command) {
requireNonNullParam(command, "command", "whileTrue");
addBinding(BindingType.RUN_WHILE_HIGH, command);
return this;
}
/**
* Starts the given command when the condition changes to `false` and cancels it when the
* condition changes to `true`.
*
* <p>Doesn't re-start the command if it ends while the condition is still `false`.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileFalse(Command command) {
requireNonNullParam(command, "command", "whileFalse");
addBinding(BindingType.RUN_WHILE_LOW, command);
return this;
}
/**
* Toggles a command when the condition changes from `false` to `true`.
*
* @param command the command to toggle
* @return this trigger, so calls can be chained
*/
public Trigger toggleOnTrue(Command command) {
requireNonNullParam(command, "command", "toggleOnTrue");
addBinding(BindingType.TOGGLE_ON_RISING_EDGE, command);
return this;
}
/**
* Toggles a command when the condition changes from `true` to `false`.
*
* @param command the command to toggle
* @return this trigger, so calls can be chained
*/
public Trigger toggleOnFalse(Command command) {
requireNonNullParam(command, "command", "toggleOnFalse");
addBinding(BindingType.TOGGLE_ON_FALLING_EDGE, command);
return this;
}
@Override
public boolean getAsBoolean() {
return m_condition.getAsBoolean();
}
/**
* Composes two triggers with logical AND.
*
* @param trigger the condition to compose with
* @return A trigger which is active when both component triggers are active.
*/
public Trigger and(BooleanSupplier trigger) {
return new Trigger(
m_scheduler, m_loop, () -> m_condition.getAsBoolean() && trigger.getAsBoolean());
}
/**
* Composes two triggers with logical OR.
*
* @param trigger the condition to compose with
* @return A trigger which is active when either component trigger is active.
*/
public Trigger or(BooleanSupplier trigger) {
return new Trigger(
m_scheduler, m_loop, () -> m_condition.getAsBoolean() || trigger.getAsBoolean());
}
/**
* Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
* negation of this trigger.
*
* @return the negated trigger
*/
public Trigger negate() {
return new Trigger(m_scheduler, m_loop, () -> !m_condition.getAsBoolean());
}
/**
* Creates a new debounced trigger from this trigger - it will become active when this trigger has
* been active for longer than the specified period.
*
* @param duration The debounce period.
* @return The debounced trigger (rising edges debounced only)
*/
public Trigger debounce(Time duration) {
return debounce(duration, Debouncer.DebounceType.kRising);
}
/**
* Creates a new debounced trigger from this trigger - it will become active when this trigger has
* been active for longer than the specified period.
*
* @param duration The debounce period.
* @param type The debounce type.
* @return The debounced trigger.
*/
public Trigger debounce(Time duration, Debouncer.DebounceType type) {
var debouncer = new Debouncer(duration.in(Seconds), type);
return new Trigger(m_scheduler, m_loop, () -> debouncer.calculate(m_condition.getAsBoolean()));
}
private void poll() {
// Clear bindings that no longer need to run
// This should always be checked, regardless of signal change, since bindings may be scoped
// and those scopes may become inactive.
clearStaleBindings();
var signal = readSignal();
if (signal == m_previousSignal) {
// No change in the signal. Nothing to do
return;
}
if (signal == Signal.HIGH) {
// Signal is now high when it wasn't before - a rising edge
scheduleBindings(BindingType.SCHEDULE_ON_RISING_EDGE);
scheduleBindings(BindingType.RUN_WHILE_HIGH);
cancelBindings(BindingType.RUN_WHILE_LOW);
toggleBindings(BindingType.TOGGLE_ON_RISING_EDGE);
}
if (signal == Signal.LOW) {
// Signal is now low when it wasn't before - a falling edge
scheduleBindings(BindingType.SCHEDULE_ON_FALLING_EDGE);
scheduleBindings(BindingType.RUN_WHILE_LOW);
cancelBindings(BindingType.RUN_WHILE_HIGH);
toggleBindings(BindingType.TOGGLE_ON_FALLING_EDGE);
}
m_previousSignal = signal;
}
private Signal readSignal() {
if (m_condition.getAsBoolean()) {
return Signal.HIGH;
} else {
return Signal.LOW;
}
}
/** Removes bindings in inactive scopes. Running commands tied to those bindings are canceled. */
private void clearStaleBindings() {
m_bindings.forEach(
(_bindingType, bindings) -> {
for (var iterator = bindings.iterator(); iterator.hasNext(); ) {
var binding = iterator.next();
if (binding.scope().active()) {
// This binding's scope is still active, leave it running
continue;
}
// The scope is no long active. Remove the binding and immediately cancel its command.
iterator.remove();
m_scheduler.cancel(binding.command());
}
});
}
/**
* Schedules all commands bound to the given binding type.
*
* @param bindingType the binding type to schedule
*/
private void scheduleBindings(BindingType bindingType) {
m_bindings.getOrDefault(bindingType, List.of()).forEach(m_scheduler::schedule);
}
/**
* Cancels all commands bound to the given binding type.
*
* @param bindingType the binding type to cancel
*/
private void cancelBindings(BindingType bindingType) {
m_bindings
.getOrDefault(bindingType, List.of())
.forEach(binding -> m_scheduler.cancel(binding.command()));
}
/**
* Toggles all commands bound to the given binding type. If a command is currently scheduled or
* running, it will be canceled; otherwise, it will be scheduled.
*
* @param bindingType the binding type to cancel
*/
private void toggleBindings(BindingType bindingType) {
m_bindings
.getOrDefault(bindingType, List.of())
.forEach(
binding -> {
var command = binding.command();
if (m_scheduler.isScheduledOrRunning(command)) {
m_scheduler.cancel(command);
} else {
m_scheduler.schedule(binding);
}
});
}
// package-private for testing
void addBinding(BindingScope scope, BindingType bindingType, Command command) {
// Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
// stack frame filtering and modification.
m_bindings
.computeIfAbsent(bindingType, _k -> new ArrayList<>())
.add(new Binding(scope, bindingType, command, new Throwable().getStackTrace()));
if (!m_isBoundToEventLoop) {
m_loop.bind(m_eventLoopCallback);
m_isBoundToEventLoop = true;
}
}
private void addBinding(BindingType bindingType, Command command) {
BindingScope scope =
switch (m_scheduler.currentCommand()) {
case Command c -> {
// A command is creating a binding - make it scoped to that specific command
yield BindingScope.forCommand(m_scheduler, c);
}
case null -> {
// Creating a binding outside a command - it's global in scope
yield BindingScope.global();
}
};
addBinding(scope, bindingType, command);
}
}

343
commandsv3/src/main/java/org/wpilib/commands3/button/CommandGenericHID.java Normal file
View File

@@ -0,0 +1,343 @@
// 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.commands3.button;
import edu.wpi.first.math.Pair;
import edu.wpi.first.wpilibj.DriverStation.POVDirection;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.event.EventLoop;
import java.util.HashMap;
import java.util.Map;
import org.wpilib.commands3.Scheduler;
import org.wpilib.commands3.Trigger;
/**
* A version of {@link GenericHID} with {@link Trigger} factories for command-based.
*
* @see GenericHID
*/
public class CommandGenericHID {
private final Scheduler m_scheduler;
private final GenericHID m_hid;
private final Map<EventLoop, Map<Integer, Trigger>> m_buttonCache = new HashMap<>();
private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisLessThanCache =
new HashMap<>();
private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisGreaterThanCache =
new HashMap<>();
private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>>
m_axisMagnitudeGreaterThanCache = new HashMap<>();
private final Map<EventLoop, Map<Integer, Trigger>> m_povCache = new HashMap<>();
/**
* Construct an instance of a device.
*
* @param scheduler The scheduler that should execute the triggered commands.
* @param port The port index on the Driver Station that the device is plugged into.
*/
public CommandGenericHID(Scheduler scheduler, int port) {
m_scheduler = scheduler;
m_hid = new GenericHID(port);
}
/**
* Construct an instance of a device.
*
* @param port The port index on the Driver Station that the device is plugged into.
*/
public CommandGenericHID(int port) {
this(Scheduler.getDefault(), port);
}
/**
* Get the underlying GenericHID object.
*
* @return the wrapped GenericHID object
*/
public GenericHID getHID() {
return m_hid;
}
/**
* Constructs an event instance around this button's digital signal.
*
* @param button the button index
* @return an event instance representing the button's digital signal attached to the {@link
* Scheduler#getDefaultEventLoop() default scheduler button loop}.
* @see #button(int, EventLoop)
*/
public Trigger button(int button) {
return button(button, m_scheduler.getDefaultEventLoop());
}
/**
* Constructs an event instance around this button's digital signal.
*
* @param button the button index
* @param loop the event loop instance to attach the event to.
* @return an event instance representing the button's digital signal attached to the given loop.
*/
public Trigger button(int button, EventLoop loop) {
var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>());
return cache.computeIfAbsent(
button, k -> new Trigger(m_scheduler, loop, () -> m_hid.getRawButton(k)));
}
/**
* Constructs a Trigger instance based around this angle of the default (index 0) POV on the HID,
* attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}.
*
* @param angle POV angle
* @return a Trigger instance based around this angle of a POV on the HID.
*/
public Trigger pov(POVDirection angle) {
return pov(0, angle, m_scheduler.getDefaultEventLoop());
}
/**
* Constructs a Trigger instance based around this angle of a POV on the HID.
*
* @param pov index of the POV to read (starting at 0). Defaults to 0.
* @param angle POV angle
* @param loop the event loop instance to attach the event to. Defaults to {@link
* Scheduler#getDefaultEventLoop() the default command scheduler button loop}.
* @return a Trigger instance based around this angle of a POV on the HID.
*/
public Trigger pov(int pov, POVDirection angle, EventLoop loop) {
var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>());
// angle.value is a 4 bit bitfield
return cache.computeIfAbsent(
pov * 16 + angle.value,
k -> new Trigger(m_scheduler, loop, () -> m_hid.getPOV(pov) == angle));
}
/**
* Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV
* on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler
* button loop}.
*
* @return a Trigger instance based around the 0 degree angle of a POV on the HID.
*/
public Trigger povUp() {
return pov(POVDirection.Up);
}
/**
* Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index
* 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command
* scheduler button loop}.
*
* @return a Trigger instance based around the 45 degree angle of a POV on the HID.
*/
public Trigger povUpRight() {
return pov(POVDirection.UpRight);
}
/**
* Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0)
* POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command
* scheduler button loop}.
*
* @return a Trigger instance based around the 90 degree angle of a POV on the HID.
*/
public Trigger povRight() {
return pov(POVDirection.Right);
}
/**
* Constructs a Trigger instance based around the 135 degree angle (right down) of the default
* (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default
* command scheduler button loop}.
*
* @return a Trigger instance based around the 135 degree angle of a POV on the HID.
*/
public Trigger povDownRight() {
return pov(POVDirection.DownRight);
}
/**
* Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0)
* POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command
* scheduler button loop}.
*
* @return a Trigger instance based around the 180 degree angle of a POV on the HID.
*/
public Trigger povDown() {
return pov(POVDirection.Down);
}
/**
* Constructs a Trigger instance based around the 225 degree angle (down left) of the default
* (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default
* command scheduler button loop}.
*
* @return a Trigger instance based around the 225 degree angle of a POV on the HID.
*/
public Trigger povDownLeft() {
return pov(POVDirection.DownLeft);
}
/**
* Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0)
* POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command
* scheduler button loop}.
*
* @return a Trigger instance based around the 270 degree angle of a POV on the HID.
*/
public Trigger povLeft() {
return pov(POVDirection.Left);
}
/**
* Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index
* 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command
* scheduler button loop}.
*
* @return a Trigger instance based around the 315 degree angle of a POV on the HID.
*/
public Trigger povUpLeft() {
return pov(POVDirection.UpLeft);
}
/**
* Constructs a Trigger instance based around the center (not pressed) position of the default
* (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default
* command scheduler button loop}.
*
* @return a Trigger instance based around the center position of a POV on the HID.
*/
public Trigger povCenter() {
return pov(POVDirection.Center);
}
/**
* Constructs a Trigger instance that is true when the axis value is less than {@code threshold},
* attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}.
*
* @param axis The axis to read, starting at 0
* @param threshold The value below which this trigger should return true.
* @return a Trigger instance that is true when the axis value is less than the provided
* threshold.
*/
public Trigger axisLessThan(int axis, double threshold) {
return axisLessThan(axis, threshold, m_scheduler.getDefaultEventLoop());
}
/**
* Constructs a Trigger instance that is true when the axis value is less than {@code threshold},
* attached to the given loop.
*
* @param axis The axis to read, starting at 0
* @param threshold The value below which this trigger should return true.
* @param loop the event loop instance to attach the trigger to
* @return a Trigger instance that is true when the axis value is less than the provided
* threshold.
*/
public Trigger axisLessThan(int axis, double threshold, EventLoop loop) {
var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>());
return cache.computeIfAbsent(
Pair.of(axis, threshold),
k -> new Trigger(m_scheduler, loop, () -> getRawAxis(axis) < threshold));
}
/**
* Constructs a Trigger instance that is true when the axis value is less than {@code threshold},
* attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}.
*
* @param axis The axis to read, starting at 0
* @param threshold The value above which this trigger should return true.
* @return a Trigger instance that is true when the axis value is greater than the provided
* threshold.
*/
public Trigger axisGreaterThan(int axis, double threshold) {
return axisGreaterThan(axis, threshold, m_scheduler.getDefaultEventLoop());
}
/**
* Constructs a Trigger instance that is true when the axis value is greater than {@code
* threshold}, attached to the given loop.
*
* @param axis The axis to read, starting at 0
* @param threshold The value above which this trigger should return true.
* @param loop the event loop instance to attach the trigger to.
* @return a Trigger instance that is true when the axis value is greater than the provided
* threshold.
*/
public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) {
var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>());
return cache.computeIfAbsent(
Pair.of(axis, threshold),
k -> new Trigger(m_scheduler, loop, () -> getRawAxis(axis) > threshold));
}
/**
* Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code
* threshold}, attached to the given loop.
*
* @param axis The axis to read, starting at 0
* @param threshold The value above which this trigger should return true.
* @param loop the event loop instance to attach the trigger to.
* @return a Trigger instance that is true when the axis magnitude value is greater than the
* provided threshold.
*/
public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) {
var cache = m_axisMagnitudeGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>());
return cache.computeIfAbsent(
Pair.of(axis, threshold),
k -> new Trigger(m_scheduler, loop, () -> Math.abs(getRawAxis(axis)) > threshold));
}
/**
* Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code
* threshold}, attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler
* button loop}.
*
* @param axis The axis to read, starting at 0
* @param threshold The value above which this trigger should return true.
* @return a Trigger instance that is true when the deadbanded axis value is active (non-zero).
*/
public Trigger axisMagnitudeGreaterThan(int axis, double threshold) {
return axisMagnitudeGreaterThan(axis, threshold, m_scheduler.getDefaultEventLoop());
}
/**
* Get the value of the axis.
*
* @param axis The axis to read, starting at 0.
* @return The value of the axis.
*/
public double getRawAxis(int axis) {
return m_hid.getRawAxis(axis);
}
/**
* Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
* right rumble.
*
* @param type Which rumble value to set
* @param value The normalized value (0 to 1) to set the rumble to
*/
public void setRumble(GenericHID.RumbleType type, double value) {
m_hid.setRumble(type, value);
}
/**
* Get if the HID is connected.
*
* @return true if the HID is connected
*/
public boolean isConnected() {
return m_hid.isConnected();
}
/**
* Gets the scheduler that should execute the triggered commands. This scheduler is set in the
* constructor, defaulting to {@link Scheduler#getDefault()} if one was not provided.
*
* @return the scheduler that should execute the triggered commands
*/
protected final Scheduler getScheduler() {
return m_scheduler;
}
}

275
commandsv3/src/main/java/org/wpilib/commands3/button/CommandJoystick.java Normal file
View File

@@ -0,0 +1,275 @@
// 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.commands3.button;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.event.EventLoop;
import org.wpilib.commands3.Scheduler;
import org.wpilib.commands3.Trigger;
/**
* A version of {@link Joystick} with {@link Trigger} factories for command-based.
*
* @see Joystick
*/
public class CommandJoystick extends CommandGenericHID {
private final Joystick m_hid;
/**
* Construct an instance of a controller.
*
* @param port The port index on the Driver Station that the controller is plugged into.
*/
public CommandJoystick(int port) {
super(port);
m_hid = new Joystick(port);
}
/**
* Construct an instance of a controller.
*
* @param scheduler The scheduler that should execute the triggered commands.
* @param port The port index on the Driver Station that the controller is plugged into.
*/
public CommandJoystick(Scheduler scheduler, int port) {
super(scheduler, port);
m_hid = new Joystick(port);
}
/**
* Get the underlying GenericHID object.
*
* @return the wrapped GenericHID object
*/
@Override
public Joystick getHID() {
return m_hid;
}
/**
* Constructs an event instance around the trigger button's digital signal.
*
* @return an event instance representing the trigger button's digital signal attached to the
* {@link Scheduler#getDefaultEventLoop() default scheduler button loop}.
* @see #trigger(EventLoop)
*/
public Trigger trigger() {
return trigger(getScheduler().getDefaultEventLoop());
}
/**
* Constructs an event instance around the trigger button's digital signal.
*
* @param loop the event loop instance to attach the event to.
* @return an event instance representing the trigger button's digital signal attached to the
* given loop.
*/
public Trigger trigger(EventLoop loop) {
return button(Joystick.ButtonType.kTrigger.value, loop);
}
/**
* Constructs an event instance around the top button's digital signal.
*
* @return an event instance representing the top button's digital signal attached to the {@link
* Scheduler#getDefaultEventLoop() default scheduler button loop}.
* @see #top(EventLoop)
*/
public Trigger top() {
return top(getScheduler().getDefaultEventLoop());
}
/**
* Constructs an event instance around the top button's digital signal.
*
* @param loop the event loop instance to attach the event to.
* @return an event instance representing the top button's digital signal attached to the given
* loop.
*/
public Trigger top(EventLoop loop) {
return button(Joystick.ButtonType.kTop.value, loop);
}
/**
* Set the channel associated with the X axis.
*
* @param channel The channel to set the axis to.
*/
public void setXChannel(int channel) {
m_hid.setXChannel(channel);
}
/**
* Set the channel associated with the Y axis.
*
* @param channel The channel to set the axis to.
*/
public void setYChannel(int channel) {
m_hid.setYChannel(channel);
}
/**
* Set the channel associated with the Z axis.
*
* @param channel The channel to set the axis to.
*/
public void setZChannel(int channel) {
m_hid.setZChannel(channel);
}
/**
* Set the channel associated with the throttle axis.
*
* @param channel The channel to set the axis to.
*/
public void setThrottleChannel(int channel) {
m_hid.setThrottleChannel(channel);
}
/**
* Set the channel associated with the twist axis.
*
* @param channel The channel to set the axis to.
*/
public void setTwistChannel(int channel) {
m_hid.setTwistChannel(channel);
}
/**
* Get the channel currently associated with the X axis.
*
* @return The channel for the axis.
*/
public int getXChannel() {
return m_hid.getXChannel();
}
/**
* Get the channel currently associated with the Y axis.
*
* @return The channel for the axis.
*/
public int getYChannel() {
return m_hid.getYChannel();
}
/**
* Get the channel currently associated with the Z axis.
*
* @return The channel for the axis.
*/
public int getZChannel() {
return m_hid.getZChannel();
}
/**
* Get the channel currently associated with the twist axis.
*
* @return The channel for the axis.
*/
public int getTwistChannel() {
return m_hid.getTwistChannel();
}
/**
* Get the channel currently associated with the throttle axis.
*
* @return The channel for the axis.
*/
public int getThrottleChannel() {
return m_hid.getThrottleChannel();
}
/**
* Get the x position of the HID.
*
* <p>This depends on the mapping of the joystick connected to the current port. On most
* joysticks, positive is to the right.
*
* @return the x position
*/
public double getX() {
return m_hid.getX();
}
/**
* Get the y position of the HID.
*
* <p>This depends on the mapping of the joystick connected to the current port. On most
* joysticks, positive is to the back.
*
* @return the y position
*/
public double getY() {
return m_hid.getY();
}
/**
* Get the z position of the HID.
*
* @return the z position
*/
public double getZ() {
return m_hid.getZ();
}
/**
* Get the twist value of the current joystick. This depends on the mapping of the joystick
* connected to the current port.
*
* @return The Twist value of the joystick.
*/
public double getTwist() {
return m_hid.getTwist();
}
/**
* Get the throttle value of the current joystick. This depends on the mapping of the joystick
* connected to the current port.
*
* @return The Throttle value of the joystick.
*/
public double getThrottle() {
return m_hid.getThrottle();
}
/**
* Get the magnitude of the vector formed by the joystick's current position relative to its
* origin.
*
* @return The magnitude of the direction vector
*/
public double getMagnitude() {
return m_hid.getMagnitude();
}
/**
* Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward
* and clockwise is positive. (Straight right is π/2.)
*
* @return The direction of the vector in radians
*/
public double getDirectionRadians() {
// https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
// A positive rotation around the X axis moves the joystick right, and a
// positive rotation around the Y axis moves the joystick backward. When
// treating them as translations, 0 radians is measured from the right
// direction, and angle increases clockwise.
//
// It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
// so that 0 radians is forward.
return m_hid.getDirectionRadians();
}
/**
* Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward
* and clockwise is positive. (Straight right is 90.)
*
* @return The direction of the vector in degrees
*/
public double getDirectionDegrees() {
return m_hid.getDirectionDegrees();
}
}

61
commandsv3/src/main/java/org/wpilib/commands3/button/InternalButton.java Normal file
View File

@@ -0,0 +1,61 @@
// 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.commands3.button;
import java.util.concurrent.atomic.AtomicBoolean;
import org.wpilib.commands3.Trigger;
/**
* This class is intended to be used within a program. The programmer can manually set its value.
* Also includes a setting for whether it should invert its value.
*/
public class InternalButton extends Trigger {
// need to be references, so they can be mutated after being captured in the constructor.
private final AtomicBoolean m_pressed;
private final AtomicBoolean m_inverted;
/** Creates an InternalButton that is not inverted. */
public InternalButton() {
this(false);
}
/**
* Creates an InternalButton which is inverted depending on the input.
*
* @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
* when set to false.
*/
public InternalButton(boolean inverted) {
this(new AtomicBoolean(), new AtomicBoolean(inverted));
}
/*
* Mock constructor so the AtomicBoolean objects can be constructed before the super
* constructor invocation.
*/
private InternalButton(AtomicBoolean state, AtomicBoolean inverted) {
super(() -> state.get() != inverted.get());
m_pressed = state;
m_inverted = inverted;
}
/**
* Sets whether to invert button state.
*
* @param inverted Whether button state should be inverted.
*/
public void setInverted(boolean inverted) {
m_inverted.set(inverted);
}
/**
* Sets whether button is pressed.
*
* @param pressed Whether button is pressed.
*/
public void setPressed(boolean pressed) {
m_pressed.set(pressed);
}
}

24
commandsv3/src/main/java/org/wpilib/commands3/button/JoystickButton.java Normal file
View File

@@ -0,0 +1,24 @@
// 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.commands3.button;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.GenericHID;
import org.wpilib.commands3.Trigger;
/** A {@link Trigger} that gets its state from a {@link GenericHID}. */
public class JoystickButton extends Trigger {
/**
* Creates a joystick button for triggering commands.
*
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
* @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
*/
public JoystickButton(GenericHID joystick, int buttonNumber) {
super(() -> joystick.getRawButton(buttonNumber));
requireNonNullParam(joystick, "joystick", "JoystickButton");
}
}

66
commandsv3/src/main/java/org/wpilib/commands3/button/NetworkButton.java Normal file
View File

@@ -0,0 +1,66 @@
// 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.commands3.button;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.BooleanTopic;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.wpilib.commands3.Trigger;
/** A {@link Trigger} that uses a {@link NetworkTable} boolean field. */
public class NetworkButton extends Trigger {
/**
* Creates a NetworkButton that commands can be bound to.
*
* @param topic The boolean topic that contains the value.
*/
public NetworkButton(BooleanTopic topic) {
this(topic.subscribe(false));
}
/**
* Creates a NetworkButton that commands can be bound to.
*
* @param sub The boolean subscriber that provides the value.
*/
public NetworkButton(BooleanSubscriber sub) {
super(() -> sub.getTopic().getInstance().isConnected() && sub.get());
requireNonNullParam(sub, "sub", "NetworkButton");
}
/**
* Creates a NetworkButton that commands can be bound to.
*
* @param table The table where the networktable value is located.
* @param field The field that is the value.
*/
public NetworkButton(NetworkTable table, String field) {
this(table.getBooleanTopic(field));
}
/**
* Creates a NetworkButton that commands can be bound to.
*
* @param table The table where the networktable value is located.
* @param field The field that is the value.
*/
public NetworkButton(String table, String field) {
this(NetworkTableInstance.getDefault(), table, field);
}
/**
* Creates a NetworkButton that commands can be bound to.
*
* @param inst The NetworkTable instance to use
* @param table The table where the networktable value is located.
* @param field The field that is the value.
*/
public NetworkButton(NetworkTableInstance inst, String table, String field) {
this(inst.getTable(table), field);
}
}

36
commandsv3/src/main/java/org/wpilib/commands3/button/POVButton.java Normal file
View File

@@ -0,0 +1,36 @@
// 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.commands3.button;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.DriverStation.POVDirection;
import edu.wpi.first.wpilibj.GenericHID;
import org.wpilib.commands3.Trigger;
/** A {@link Trigger} that gets its state from a POV on a {@link GenericHID}. */
public class POVButton extends Trigger {
/**
* Creates a POV button for triggering commands.
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle
* @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
*/
public POVButton(GenericHID joystick, POVDirection angle, int povNumber) {
super(() -> joystick.getPOV(povNumber) == angle);
requireNonNullParam(joystick, "joystick", "POVButton");
}
/**
* Creates a POV button for triggering commands. By default, acts on POV 0
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle
*/
public POVButton(GenericHID joystick, POVDirection angle) {
this(joystick, angle, 0);
}
}

53
commandsv3/src/main/java/org/wpilib/commands3/button/RobotModeTriggers.java Normal file
View File

@@ -0,0 +1,53 @@
// 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.commands3.button;
import edu.wpi.first.wpilibj.DriverStation;
import org.wpilib.commands3.Trigger;
/**
* A class containing static {@link Trigger} factories for running callbacks when the robot mode
* changes.
*/
public final class RobotModeTriggers {
// Utility class
private RobotModeTriggers() {}
/**
* Returns a trigger that is true when the robot is enabled in autonomous mode.
*
* @return A trigger that is true when the robot is enabled in autonomous mode.
*/
public static Trigger autonomous() {
return new Trigger(DriverStation::isAutonomousEnabled);
}
/**
* Returns a trigger that is true when the robot is enabled in teleop mode.
*
* @return A trigger that is true when the robot is enabled in teleop mode.
*/
public static Trigger teleop() {
return new Trigger(DriverStation::isTeleopEnabled);
}
/**
* Returns a trigger that is true when the robot is disabled.
*
* @return A trigger that is true when the robot is disabled.
*/
public static Trigger disabled() {
return new Trigger(DriverStation::isDisabled);
}
/**
* Returns a trigger that is true when the robot is enabled in test mode.
*
* @return A trigger that is true when the robot is enabled in test mode.
*/
public static Trigger test() {
return new Trigger(DriverStation::isTestEnabled);
}
}

65
commandsv3/src/main/java/org/wpilib/commands3/proto/CommandProto.java Normal file
View File

@@ -0,0 +1,65 @@
// 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.commands3.proto;
import edu.wpi.first.util.protobuf.Protobuf;
import org.wpilib.commands3.Command;
import org.wpilib.commands3.Mechanism;
import org.wpilib.commands3.Scheduler;
import org.wpilib.commands3.proto.ProtobufCommands.ProtobufCommand;
import us.hebi.quickbuf.Descriptors;
/** Protobuf serde for running commands. */
public class CommandProto implements Protobuf<Command, ProtobufCommand> {
private final Scheduler m_scheduler;
public CommandProto(Scheduler scheduler) {
m_scheduler = scheduler;
}
@Override
public Class<Command> getTypeClass() {
return Command.class;
}
@Override
public Descriptors.Descriptor getDescriptor() {
return ProtobufCommand.getDescriptor();
}
@Override
public ProtobufCommand createMessage() {
return ProtobufCommand.newInstance();
}
@Override
public Command unpack(ProtobufCommand msg) {
// Not possible. The command behavior is what really matters, and it cannot be serialized
throw new UnsupportedOperationException("Deserialization not supported");
}
@Override
public void pack(ProtobufCommand msg, Command command) {
msg.clear();
msg.setId(m_scheduler.runId(command));
Command parent = m_scheduler.getParentOf(command);
if (parent != null) {
msg.setParentId(m_scheduler.runId(parent));
}
msg.setName(command.name());
msg.setPriority(command.priority());
Protobuf.packArray(
msg.getMutableRequirements(),
command.requirements().toArray(new Mechanism[0]),
new MechanismProto());
if (m_scheduler.isRunning(command)) {
msg.setLastTimeMs(m_scheduler.lastCommandRuntimeMs(command));
msg.setTotalTimeMs(m_scheduler.totalRuntimeMs(command));
}
}
}

38
commandsv3/src/main/java/org/wpilib/commands3/proto/MechanismProto.java Normal file
View File

@@ -0,0 +1,38 @@
// 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.commands3.proto;
import edu.wpi.first.util.protobuf.Protobuf;
import org.wpilib.commands3.Mechanism;
import org.wpilib.commands3.proto.ProtobufCommands.ProtobufMechanism;
import us.hebi.quickbuf.Descriptors;
public class MechanismProto implements Protobuf<Mechanism, ProtobufMechanism> {
@Override
public Class<Mechanism> getTypeClass() {
return Mechanism.class;
}
@Override
public Descriptors.Descriptor getDescriptor() {
return ProtobufMechanism.getDescriptor();
}
@Override
public ProtobufMechanism createMessage() {
return ProtobufMechanism.newInstance();
}
@Override
public Mechanism unpack(ProtobufMechanism msg) {
throw new UnsupportedOperationException("Deserialization not supported");
}
@Override
public void pack(ProtobufMechanism msg, Mechanism value) {
msg.clear();
msg.setName(value.getName());
}
}

58
commandsv3/src/main/java/org/wpilib/commands3/proto/SchedulerProto.java Normal file
View File

@@ -0,0 +1,58 @@
// 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.commands3.proto;
import edu.wpi.first.util.protobuf.Protobuf;
import org.wpilib.commands3.Command;
import org.wpilib.commands3.Scheduler;
import org.wpilib.commands3.proto.ProtobufCommands.ProtobufScheduler;
import us.hebi.quickbuf.Descriptors;
/**
* Serializes a {@link Scheduler} to a protobuf message. Deserialization is not supported. A
* serialized message will include information about commands that are currently running or
* scheduled (but not yet started), as well as how long the most recent call to {@link
* Scheduler#run()} took to execute.
*/
public class SchedulerProto implements Protobuf<Scheduler, ProtobufScheduler> {
@Override
public Class<Scheduler> getTypeClass() {
return Scheduler.class;
}
@Override
public Descriptors.Descriptor getDescriptor() {
return ProtobufScheduler.getDescriptor();
}
@Override
public ProtobufScheduler createMessage() {
return ProtobufScheduler.newInstance();
}
@Override
public Scheduler unpack(ProtobufScheduler msg) {
throw new UnsupportedOperationException("Deserialization not supported");
}
@Override
public void pack(ProtobufScheduler msg, Scheduler scheduler) {
msg.clear();
var commandProto = new CommandProto(scheduler);
Protobuf.packArray(
msg.getMutableQueuedCommands(),
scheduler.getQueuedCommands().toArray(new Command[0]),
commandProto);
Protobuf.packArray(
msg.getMutableRunningCommands(),
scheduler.getRunningCommands().toArray(new Command[0]),
commandProto);
msg.setLastTimeMs(scheduler.lastRuntimeMs());
}
}