SCRIPT Move java files

commandsv2/src/main/java/org/wpilib/command2/Command.java

@@ -0,0 +1,650 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import 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.util.function.BooleanConsumer;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.Collection;
+import java.util.HashSet;
+import java.util.Set;
+import java.util.function.BooleanSupplier;
+import org.wpilib.annotation.NoDiscard;
+
+/**
+ * A state machine representing a complete action to be performed by the robot. Commands are run by
+ * the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to build
+ * complicated multistep actions without the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
+ * specified explicitly from the command implementation.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+@NoDiscard("Commands must be used! Did you mean to bind it to a trigger?")
+public abstract class Command implements Sendable {
+  /** Requirements set. */
+  private final Set<Subsystem> m_requirements = new HashSet<>();
+
+  /** Default constructor. */
+  @SuppressWarnings("this-escape")
+  protected Command() {
+    String name = getClass().getName();
+    SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
+  }
+
+  /** The initial subroutine of a command. Called once when the command is initially scheduled. */
+  public void initialize() {}
+
+  /** The main body of a command. Called repeatedly while the command is scheduled. */
+  public void execute() {}
+
+  /**
+   * The action to take when the command ends. Called when either the command finishes normally, or
+   * when it interrupted/canceled.
+   *
+   * <p>Do not schedule commands here that share requirements with this command. Use {@link
+   * #andThen(Command...)} instead.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  public void end(boolean interrupted) {}
+
+  /**
+   * Whether the command has finished. Once a command finishes, the scheduler will call its end()
+   * method and un-schedule it.
+   *
+   * @return whether the command has finished.
+   */
+  public boolean isFinished() {
+    return false;
+  }
+
+  /**
+   * Specifies the set of subsystems used by this command. Two commands cannot use the same
+   * subsystem at the same time. If another command is scheduled that shares a requirement, {@link
+   * #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return
+   * an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the requirements as a field, and
+   * return that field here, rather than allocating a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   * @see InterruptionBehavior
+   */
+  public Set<Subsystem> getRequirements() {
+    return m_requirements;
+  }
+
+  /**
+   * Adds the specified subsystems to the requirements of the command. The scheduler will prevent
+   * two commands that require the same subsystem from being scheduled simultaneously.
+   *
+   * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
+   * this method should normally be called from the command's constructor.
+   *
+   * @param requirements the requirements to add
+   */
+  public final void addRequirements(Subsystem... requirements) {
+    for (Subsystem requirement : requirements) {
+      m_requirements.add(requireNonNullParam(requirement, "requirement", "addRequirements"));
+    }
+  }
+
+  /**
+   * Adds the specified subsystems to the requirements of the command. The scheduler will prevent
+   * two commands that require the same subsystem from being scheduled simultaneously.
+   *
+   * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
+   * this method should normally be called from the command's constructor.
+   *
+   * @param requirements the requirements to add
+   */
+  public final void addRequirements(Collection<Subsystem> requirements) {
+    m_requirements.addAll(requirements);
+  }
+
+  /**
+   * Gets the name of this Command.
+   *
+   * <p>By default, the simple class name is used. This can be changed with {@link
+   * #setName(String)}.
+   *
+   * @return The display name of the Command
+   */
+  public String getName() {
+    return SendableRegistry.getName(this);
+  }
+
+  /**
+   * Sets the name of this Command.
+   *
+   * @param name The display name of the Command.
+   */
+  public void setName(String name) {
+    SendableRegistry.setName(this, name);
+  }
+
+  /**
+   * Gets the subsystem name of this Command.
+   *
+   * @return Subsystem name
+   */
+  public String getSubsystem() {
+    return SendableRegistry.getSubsystem(this);
+  }
+
+  /**
+   * Sets the subsystem name of this Command.
+   *
+   * @param subsystem subsystem name
+   */
+  public void setSubsystem(String subsystem) {
+    SendableRegistry.setSubsystem(this, subsystem);
+  }
+
+  /**
+   * Decorates this command with a timeout. If the specified timeout is exceeded before the command
+   * finishes normally, the command will be interrupted and un-scheduled.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param seconds the timeout duration
+   * @return the command with the timeout added
+   */
+  public ParallelRaceGroup withTimeout(double seconds) {
+    return raceWith(new WaitCommand(seconds));
+  }
+
+  /**
+   * Decorates this command with a timeout. If the specified timeout is exceeded before the command
+   * finishes normally, the command will be interrupted and un-scheduled.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param time the timeout duration
+   * @return the command with the timeout added
+   */
+  public ParallelRaceGroup withTimeout(Time time) {
+    return withTimeout(time.in(Seconds));
+  }
+
+  /**
+   * Decorates this command with an interrupt condition. If the specified condition becomes true
+   * before the command finishes normally, the command will be interrupted and un-scheduled.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   * @see #onlyWhile(BooleanSupplier)
+   */
+  public ParallelRaceGroup until(BooleanSupplier condition) {
+    return raceWith(new WaitUntilCommand(condition));
+  }
+
+  /**
+   * Decorates this command with a run condition. If the specified condition becomes false before
+   * the command finishes normally, the command will be interrupted and un-scheduled.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param condition the run condition
+   * @return the command with the run condition added
+   * @see #until(BooleanSupplier)
+   */
+  public ParallelRaceGroup onlyWhile(BooleanSupplier condition) {
+    return until(() -> !condition.getAsBoolean());
+  }
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  public SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
+    return beforeStarting(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * Decorates this command with another command to run before this command starts.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  public SequentialCommandGroup beforeStarting(Command before) {
+    return new SequentialCommandGroup(before, this);
+  }
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  public SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
+    return andThen(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * Decorates this command with a set of commands to run after it in sequence. Often more
+   * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param next the commands to run next
+   * @return the decorated command
+   */
+  public SequentialCommandGroup andThen(Command... next) {
+    SequentialCommandGroup group = new SequentialCommandGroup(this);
+    group.addCommands(next);
+    return group;
+  }
+
+  /**
+   * Creates a new command that runs this command and the deadline in parallel, finishing (and
+   * interrupting this command) when the deadline finishes.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see Command#deadlineFor
+   */
+  public ParallelDeadlineGroup withDeadline(Command deadline) {
+    return new ParallelDeadlineGroup(deadline, this);
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the calling
+   * command ends and interrupting all the others. Often more convenient/less-verbose than
+   * constructing a new {@link ParallelDeadlineGroup} explicitly.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   * @deprecated Use {@link deadlineFor} instead.
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public ParallelDeadlineGroup deadlineWith(Command... parallel) {
+    return new ParallelDeadlineGroup(this, parallel);
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the calling
+   * command ends and interrupting all the others. Often more convenient/less-verbose than
+   * constructing a new {@link ParallelDeadlineGroup} explicitly.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel. Note the parallel commands will be interrupted
+   *     when the deadline command ends
+   * @return the decorated command
+   * @see Command#withDeadline
+   */
+  public ParallelDeadlineGroup deadlineFor(Command... parallel) {
+    return new ParallelDeadlineGroup(this, parallel);
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the last
+   * command ends. Often more convenient/less-verbose than constructing a new {@link
+   * ParallelCommandGroup} explicitly.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  public ParallelCommandGroup alongWith(Command... parallel) {
+    ParallelCommandGroup group = new ParallelCommandGroup(this);
+    group.addCommands(parallel);
+    return group;
+  }
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending when the first
+   * command ends. Often more convenient/less-verbose than constructing a new {@link
+   * ParallelRaceGroup} explicitly.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  public ParallelRaceGroup raceWith(Command... parallel) {
+    ParallelRaceGroup group = new ParallelRaceGroup(this);
+    group.addCommands(parallel);
+    return group;
+  }
+
+  /**
+   * Decorates this command to run repeatedly, restarting it when it ends, until this command is
+   * interrupted. The decorated command can still be canceled.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @return the decorated command
+   */
+  public RepeatCommand repeatedly() {
+    return new RepeatCommand(this);
+  }
+
+  /**
+   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. Use this for
+   * "forking off" from command compositions when the user does not wish to extend the command's
+   * requirements to the entire command composition. ProxyCommand has unique implications and
+   * semantics, see the WPILib docs for a full explanation.
+   *
+   * @return the decorated command
+   * @see ProxyCommand
+   * @see <a
+   *     href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">WPILib
+   *     docs</a>
+   */
+  public ProxyCommand asProxy() {
+    return new ProxyCommand(this);
+  }
+
+  /**
+   * Decorates this command to only run if this condition is not met. If the command is already
+   * running and the condition changes to true, the command will not stop running. The requirements
+   * of this command will be kept for the new conditional command.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param condition the condition that will prevent the command from running
+   * @return the decorated command
+   * @see #onlyIf(BooleanSupplier)
+   */
+  public ConditionalCommand unless(BooleanSupplier condition) {
+    return new ConditionalCommand(new InstantCommand(), this, condition);
+  }
+
+  /**
+   * Decorates this command to only run if this condition is met. If the command is already running
+   * and the condition changes to false, the command will not stop running. The requirements of this
+   * command will be kept for the new conditional command.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param condition the condition that will allow the command to run
+   * @return the decorated command
+   * @see #unless(BooleanSupplier)
+   */
+  public ConditionalCommand onlyIf(BooleanSupplier condition) {
+    return unless(() -> !condition.getAsBoolean());
+  }
+
+  /**
+   * Decorates this command to run or stop when disabled.
+   *
+   * @param doesRunWhenDisabled true to run when disabled.
+   * @return the decorated command
+   */
+  public WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) {
+    return new WrapperCommand(this) {
+      @Override
+      public boolean runsWhenDisabled() {
+        return doesRunWhenDisabled;
+      }
+    };
+  }
+
+  /**
+   * Decorates this command to have a different {@link InterruptionBehavior interruption behavior}.
+   *
+   * @param interruptBehavior the desired interrupt behavior
+   * @return the decorated command
+   */
+  public WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
+    return new WrapperCommand(this) {
+      @Override
+      public InterruptionBehavior getInterruptionBehavior() {
+        return interruptBehavior;
+      }
+    };
+  }
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following the command's
+   * inherent {@link #end(boolean)} method.
+   *
+   * @param end a lambda accepting a boolean parameter specifying whether the command was
+   *     interrupted.
+   * @return the decorated command
+   */
+  public WrapperCommand finallyDo(BooleanConsumer end) {
+    requireNonNullParam(end, "end", "Command.finallyDo()");
+    return new WrapperCommand(this) {
+      @Override
+      public void end(boolean interrupted) {
+        super.end(interrupted);
+        end.accept(interrupted);
+      }
+    };
+  }
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following the command's
+   * inherent {@link #end(boolean)} method. The provided lambda will run identically in both
+   * interrupt and end cases.
+   *
+   * @param end a lambda to run when the command ends, whether or not it was interrupted.
+   * @return the decorated command
+   */
+  public WrapperCommand finallyDo(Runnable end) {
+    return finallyDo(interrupted -> end.run());
+  }
+
+  /**
+   * Decorates this command with a lambda to call on interrupt, following the command's inherent
+   * {@link #end(boolean)} method.
+   *
+   * @param handler a lambda to run when the command is interrupted
+   * @return the decorated command
+   */
+  public WrapperCommand handleInterrupt(Runnable handler) {
+    requireNonNullParam(handler, "handler", "Command.handleInterrupt()");
+    return finallyDo(
+        interrupted -> {
+          if (interrupted) {
+            handler.run();
+          }
+        });
+  }
+
+  /**
+   * Schedules this command.
+   *
+   * @deprecated Use CommandScheduler.getInstance().schedule(Command...) instead
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public void schedule() {
+    CommandScheduler.getInstance().schedule(this);
+  }
+
+  /**
+   * Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled
+   * regardless of {@link InterruptionBehavior interruption behavior}.
+   *
+   * @see CommandScheduler#cancel(Command...)
+   */
+  public void cancel() {
+    CommandScheduler.getInstance().cancel(this);
+  }
+
+  /**
+   * Whether the command is currently scheduled. Note that this does not detect whether the command
+   * is in a composition, only whether it is directly being run by the scheduler.
+   *
+   * @return Whether the command is scheduled.
+   */
+  public boolean isScheduled() {
+    return CommandScheduler.getInstance().isScheduled(this);
+  }
+
+  /**
+   * Whether the command requires a given subsystem.
+   *
+   * @param requirement the subsystem to inquire about
+   * @return whether the subsystem is required
+   */
+  public boolean hasRequirement(Subsystem requirement) {
+    return getRequirements().contains(requirement);
+  }
+
+  /**
+   * How the command behaves when another command with a shared requirement is scheduled.
+   *
+   * @return a variant of {@link InterruptionBehavior}, defaulting to {@link
+   *     InterruptionBehavior#kCancelSelf kCancelSelf}.
+   */
+  public InterruptionBehavior getInterruptionBehavior() {
+    return InterruptionBehavior.kCancelSelf;
+  }
+
+  /**
+   * Whether the given command should run when the robot is disabled. Override to return true if the
+   * command should run when disabled.
+   *
+   * @return whether the command should run when the robot is disabled
+   */
+  public boolean runsWhenDisabled() {
+    return false;
+  }
+
+  /**
+   * Decorates this Command with a name.
+   *
+   * @param name name
+   * @return the decorated Command
+   */
+  public WrapperCommand withName(String name) {
+    WrapperCommand wrapper = new WrapperCommand(Command.this) {};
+    wrapper.setName(name);
+    return wrapper;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Command");
+    builder.addStringProperty(".name", this::getName, null);
+    builder.addBooleanProperty(
+        "running",
+        this::isScheduled,
+        value -> {
+          if (value) {
+            if (!isScheduled()) {
+              CommandScheduler.getInstance().schedule(this);
+            }
+          } else {
+            if (isScheduled()) {
+              cancel();
+            }
+          }
+        });
+    builder.addBooleanProperty(
+        ".isParented", () -> CommandScheduler.getInstance().isComposed(this), null);
+    builder.addStringProperty(
+        "interruptBehavior", () -> getInterruptionBehavior().toString(), null);
+    builder.addBooleanProperty("runsWhenDisabled", this::runsWhenDisabled, null);
+  }
+
+  /**
+   * An enum describing the command's behavior when another command with a shared requirement is
+   * scheduled.
+   */
+  public enum InterruptionBehavior {
+    /**
+     * This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
+     * scheduled normally.
+     *
+     * <p>This is the default behavior.
+     */
+    kCancelSelf,
+    /** This command continues, and the incoming command is not scheduled. */
+    kCancelIncoming
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java

@@ -0,0 +1,782 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Watchdog;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.io.PrintWriter;
+import java.io.StringWriter;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Iterator;
+import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Optional;
+import java.util.Set;
+import java.util.WeakHashMap;
+import java.util.function.BiConsumer;
+import java.util.function.Consumer;
+
+/**
+ * The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
+ * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands
+ * synchronously from the main loop. Subsystems should be registered with the scheduler using {@link
+ * CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link Subsystem#periodic()}
+ * methods to be called and for their default commands to be scheduled.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public final class CommandScheduler implements Sendable, AutoCloseable {
+  /** The Singleton Instance. */
+  private static CommandScheduler instance;
+
+  /**
+   * Returns the Scheduler instance.
+   *
+   * @return the instance
+   */
+  public static synchronized CommandScheduler getInstance() {
+    if (instance == null) {
+      instance = new CommandScheduler();
+    }
+    return instance;
+  }
+
+  private static final Optional<Command> kNoInterruptor = Optional.empty();
+
+  private final Map<Command, Exception> m_composedCommands = new WeakHashMap<>();
+
+  // A set of the currently-running commands.
+  private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
+
+  // A map from required subsystems to their requiring commands. Also used as a set of the
+  // currently-required subsystems.
+  private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
+
+  // A map from subsystems registered with the scheduler to their default commands.  Also used
+  // as a list of currently-registered subsystems.
+  private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
+
+  private final EventLoop m_defaultButtonLoop = new EventLoop();
+  // The set of currently-registered buttons that will be polled every iteration.
+  private EventLoop m_activeButtonLoop = m_defaultButtonLoop;
+
+  private boolean m_disabled;
+
+  // Lists of user-supplied actions to be executed on scheduling events for every command.
+  private final List<Consumer<Command>> m_initActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
+  private final List<BiConsumer<Command, Optional<Command>>> m_interruptActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
+
+  // Flag and queues for avoiding ConcurrentModificationException if commands are
+  // scheduled/canceled during run
+  private boolean m_inRunLoop;
+  private final Set<Command> m_toSchedule = new LinkedHashSet<>();
+  private final List<Command> m_toCancelCommands = new ArrayList<>();
+  private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>();
+  private final Set<Command> m_endingCommands = new LinkedHashSet<>();
+
+  private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
+
+  CommandScheduler() {
+    HAL.reportUsage("CommandScheduler", "");
+    SendableRegistry.add(this, "Scheduler");
+  }
+
+  /**
+   * Changes the period of the loop overrun watchdog. This should be kept in sync with the
+   * TimedRobot period.
+   *
+   * @param period Period in seconds.
+   */
+  public void setPeriod(double period) {
+    m_watchdog.setTimeout(period);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Get the default button poll.
+   *
+   * @return a reference to the default {@link EventLoop} object polling buttons.
+   */
+  public EventLoop getDefaultButtonLoop() {
+    return m_defaultButtonLoop;
+  }
+
+  /**
+   * Get the active button poll.
+   *
+   * @return a reference to the current {@link EventLoop} object polling buttons.
+   */
+  public EventLoop getActiveButtonLoop() {
+    return m_activeButtonLoop;
+  }
+
+  /**
+   * Replace the button poll with another one.
+   *
+   * @param loop the new button polling loop object.
+   */
+  public void setActiveButtonLoop(EventLoop loop) {
+    m_activeButtonLoop =
+        requireNonNullParam(loop, "loop", "CommandScheduler" + ".replaceButtonEventLoop");
+  }
+
+  /**
+   * Initializes a given command, adds its requirements to the list, and performs the init actions.
+   *
+   * @param command The command to initialize
+   * @param requirements The command requirements
+   */
+  private void initCommand(Command command, Set<Subsystem> requirements) {
+    m_scheduledCommands.add(command);
+    for (Subsystem requirement : requirements) {
+      m_requirements.put(requirement, command);
+    }
+    command.initialize();
+    for (Consumer<Command> action : m_initActions) {
+      action.accept(command);
+    }
+
+    m_watchdog.addEpoch(command.getName() + ".initialize()");
+  }
+
+  /**
+   * Schedules a command for execution. Does nothing if the command is already scheduled. If a
+   * command's requirements are not available, it will only be started if all the commands currently
+   * using those requirements have been scheduled as interruptible. If this is the case, they will
+   * be interrupted and the command will be scheduled.
+   *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
+   * @param command the command to schedule. If null, no-op.
+   */
+  private void schedule(Command command) {
+    if (command == null) {
+      DriverStation.reportWarning("Tried to schedule a null command", true);
+      return;
+    }
+    if (m_inRunLoop) {
+      m_toSchedule.add(command);
+      return;
+    }
+
+    requireNotComposed(command);
+
+    // Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
+    // run when disabled, or the command is already scheduled.
+    if (m_disabled
+        || isScheduled(command)
+        || RobotState.isDisabled() && !command.runsWhenDisabled()) {
+      return;
+    }
+
+    Set<Subsystem> requirements = command.getRequirements();
+
+    // Schedule the command if the requirements are not currently in-use.
+    if (Collections.disjoint(m_requirements.keySet(), requirements)) {
+      initCommand(command, requirements);
+    } else {
+      // Else check if the requirements that are in use have all have interruptible commands,
+      // and if so, interrupt those commands and schedule the new command.
+      for (Subsystem requirement : requirements) {
+        Command requiring = requiring(requirement);
+        if (requiring != null
+            && requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
+          return;
+        }
+      }
+      for (Subsystem requirement : requirements) {
+        Command requiring = requiring(requirement);
+        if (requiring != null) {
+          cancel(requiring, Optional.of(command));
+        }
+      }
+      initCommand(command, requirements);
+    }
+  }
+
+  /**
+   * Schedules multiple commands for execution. Does nothing for commands already scheduled.
+   *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
+   * @param commands the commands to schedule. No-op on null.
+   */
+  public void schedule(Command... commands) {
+    for (Command command : commands) {
+      schedule(command);
+    }
+  }
+
+  /**
+   * Runs a single iteration of the scheduler. The execution occurs in the following order:
+   *
+   * <p>Subsystem periodic methods are called.
+   *
+   * <p>Button bindings are polled, and new commands are scheduled from them.
+   *
+   * <p>Currently-scheduled commands are executed.
+   *
+   * <p>End conditions are checked on currently-scheduled commands, and commands that are finished
+   * have their end methods called and are removed.
+   *
+   * <p>Any subsystems not being used as requirements have their default methods started.
+   */
+  public void run() {
+    if (m_disabled) {
+      return;
+    }
+    m_watchdog.reset();
+
+    // Run the periodic method of all registered subsystems.
+    for (Subsystem subsystem : m_subsystems.keySet()) {
+      subsystem.periodic();
+      if (RobotBase.isSimulation()) {
+        subsystem.simulationPeriodic();
+      }
+      m_watchdog.addEpoch(subsystem.getName() + ".periodic()");
+    }
+
+    // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from
+    // inside the button bindings.
+    EventLoop loopCache = m_activeButtonLoop;
+    // Poll buttons for new commands to add.
+    loopCache.poll();
+    m_watchdog.addEpoch("buttons.run()");
+
+    m_inRunLoop = true;
+    boolean isDisabled = RobotState.isDisabled();
+    // Run scheduled commands, remove finished commands.
+    for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
+      Command command = iterator.next();
+
+      if (isDisabled && !command.runsWhenDisabled()) {
+        cancel(command, kNoInterruptor);
+        continue;
+      }
+
+      command.execute();
+      for (Consumer<Command> action : m_executeActions) {
+        action.accept(command);
+      }
+      m_watchdog.addEpoch(command.getName() + ".execute()");
+      if (command.isFinished()) {
+        m_endingCommands.add(command);
+        command.end(false);
+        for (Consumer<Command> action : m_finishActions) {
+          action.accept(command);
+        }
+        m_endingCommands.remove(command);
+        iterator.remove();
+
+        m_requirements.keySet().removeAll(command.getRequirements());
+        m_watchdog.addEpoch(command.getName() + ".end(false)");
+      }
+    }
+    m_inRunLoop = false;
+
+    // Schedule/cancel commands from queues populated during loop
+    for (Command command : m_toSchedule) {
+      schedule(command);
+    }
+
+    for (int i = 0; i < m_toCancelCommands.size(); i++) {
+      cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i));
+    }
+
+    m_toSchedule.clear();
+    m_toCancelCommands.clear();
+    m_toCancelInterruptors.clear();
+
+    // Add default commands for un-required registered subsystems.
+    for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
+      if (!m_requirements.containsKey(subsystemCommand.getKey())
+          && subsystemCommand.getValue() != null) {
+        schedule(subsystemCommand.getValue());
+      }
+    }
+
+    m_watchdog.disable();
+    if (m_watchdog.isExpired()) {
+      System.out.println("CommandScheduler loop overrun");
+      m_watchdog.printEpochs();
+    }
+  }
+
+  /**
+   * Registers subsystems with the scheduler. This must be called for the subsystem's periodic block
+   * to run when the scheduler is run, and for the subsystem's default command to be scheduled. It
+   * is recommended to call this from the constructor of your subsystem implementations.
+   *
+   * @param subsystems the subsystem to register
+   */
+  public void registerSubsystem(Subsystem... subsystems) {
+    for (Subsystem subsystem : subsystems) {
+      if (subsystem == null) {
+        DriverStation.reportWarning("Tried to register a null subsystem", true);
+        continue;
+      }
+      if (m_subsystems.containsKey(subsystem)) {
+        DriverStation.reportWarning("Tried to register an already-registered subsystem", true);
+        continue;
+      }
+      m_subsystems.put(subsystem, null);
+    }
+  }
+
+  /**
+   * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
+   * block called, and will not have its default command scheduled.
+   *
+   * @param subsystems the subsystem to un-register
+   */
+  public void unregisterSubsystem(Subsystem... subsystems) {
+    m_subsystems.keySet().removeAll(Set.of(subsystems));
+  }
+
+  /**
+   * Un-registers all registered Subsystems with the scheduler. All currently registered subsystems
+   * will no longer have their periodic block called, and will not have their default command
+   * scheduled.
+   */
+  public void unregisterAllSubsystems() {
+    m_subsystems.clear();
+  }
+
+  /**
+   * Sets the default command for a subsystem. Registers that subsystem if it is not already
+   * registered. Default commands will run whenever there is no other command currently scheduled
+   * that requires the subsystem. Default commands should be written to never end (i.e. their {@link
+   * Command#isFinished()} method should return false), as they would simply be re-scheduled if they
+   * do. Default commands must also require their subsystem.
+   *
+   * @param subsystem the subsystem whose default command will be set
+   * @param defaultCommand the default command to associate with the subsystem
+   */
+  public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
+    if (subsystem == null) {
+      DriverStation.reportWarning("Tried to set a default command for a null subsystem", true);
+      return;
+    }
+    if (defaultCommand == null) {
+      DriverStation.reportWarning("Tried to set a null default command", true);
+      return;
+    }
+
+    requireNotComposed(defaultCommand);
+
+    if (!defaultCommand.getRequirements().contains(subsystem)) {
+      throw new IllegalArgumentException("Default commands must require their subsystem!");
+    }
+
+    if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
+      DriverStation.reportWarning(
+          "Registering a non-interruptible default command!\n"
+              + "This will likely prevent any other commands from requiring this subsystem.",
+          true);
+      // Warn, but allow -- there might be a use case for this.
+    }
+
+    m_subsystems.put(subsystem, defaultCommand);
+  }
+
+  /**
+   * Removes the default command for a subsystem. The current default command will run until another
+   * command is scheduled that requires the subsystem, at which point the current default command
+   * will not be re-scheduled.
+   *
+   * @param subsystem the subsystem whose default command will be removed
+   */
+  public void removeDefaultCommand(Subsystem subsystem) {
+    if (subsystem == null) {
+      DriverStation.reportWarning("Tried to remove a default command for a null subsystem", true);
+      return;
+    }
+
+    m_subsystems.put(subsystem, null);
+  }
+
+  /**
+   * Gets the default command associated with this subsystem. Null if this subsystem has no default
+   * command associated with it.
+   *
+   * @param subsystem the subsystem to inquire about
+   * @return the default command associated with the subsystem
+   */
+  public Command getDefaultCommand(Subsystem subsystem) {
+    return m_subsystems.get(subsystem);
+  }
+
+  /**
+   * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method of the
+   * canceled command with {@code true}, indicating they were canceled (as opposed to finishing
+   * normally).
+   *
+   * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}.
+   *
+   * @param commands the commands to cancel
+   */
+  public void cancel(Command... commands) {
+    for (Command command : commands) {
+      cancel(command, kNoInterruptor);
+    }
+  }
+
+  /**
+   * Cancels a command. The scheduler will only call {@link Command#end(boolean)} method of the
+   * canceled command with {@code true}, indicating they were canceled (as opposed to finishing
+   * normally).
+   *
+   * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}.
+   *
+   * @param command the command to cancel
+   * @param interruptor the interrupting command, if any
+   */
+  private void cancel(Command command, Optional<Command> interruptor) {
+    if (command == null) {
+      DriverStation.reportWarning("Tried to cancel a null command", true);
+      return;
+    }
+    if (m_endingCommands.contains(command)) {
+      return;
+    }
+    if (m_inRunLoop) {
+      m_toCancelCommands.add(command);
+      m_toCancelInterruptors.add(interruptor);
+      return;
+    }
+    if (!isScheduled(command)) {
+      return;
+    }
+
+    m_endingCommands.add(command);
+    command.end(true);
+    for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) {
+      action.accept(command, interruptor);
+    }
+    m_endingCommands.remove(command);
+    m_scheduledCommands.remove(command);
+    m_requirements.keySet().removeAll(command.getRequirements());
+    m_watchdog.addEpoch(command.getName() + ".end(true)");
+  }
+
+  /** Cancels all commands that are currently scheduled. */
+  public void cancelAll() {
+    // Copy to array to avoid concurrent modification.
+    cancel(m_scheduledCommands.toArray(new Command[0]));
+  }
+
+  /**
+   * Whether the given commands are running. Note that this only works on commands that are directly
+   * scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler
+   * does not see them.
+   *
+   * @param commands multiple commands to check
+   * @return whether all of the commands are currently scheduled
+   */
+  public boolean isScheduled(Command... commands) {
+    for (var cmd : commands) {
+      if (!isScheduled(cmd)) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Whether the given commands are running. Note that this only works on commands that are directly
+   * scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler
+   * does not see them.
+   *
+   * @param command a single command to check
+   * @return whether all of the commands are currently scheduled
+   */
+  public boolean isScheduled(Command command) {
+    return m_scheduledCommands.contains(command);
+  }
+
+  /**
+   * Returns the command currently requiring a given subsystem. Null if no command is currently
+   * requiring the subsystem
+   *
+   * @param subsystem the subsystem to be inquired about
+   * @return the command currently requiring the subsystem, or null if no command is currently
+   *     scheduled
+   */
+  public Command requiring(Subsystem subsystem) {
+    return m_requirements.get(subsystem);
+  }
+
+  /** Disables the command scheduler. */
+  public void disable() {
+    m_disabled = true;
+  }
+
+  /** Enables the command scheduler. */
+  public void enable() {
+    m_disabled = false;
+  }
+
+  /** Prints list of epochs added so far and their times. */
+  public void printWatchdogEpochs() {
+    m_watchdog.printEpochs();
+  }
+
+  /**
+   * Adds an action to perform on the initialization of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandInitialize(Consumer<Command> action) {
+    m_initActions.add(requireNonNullParam(action, "action", "onCommandInitialize"));
+  }
+
+  /**
+   * Adds an action to perform on the execution of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandExecute(Consumer<Command> action) {
+    m_executeActions.add(requireNonNullParam(action, "action", "onCommandExecute"));
+  }
+
+  /**
+   * Adds an action to perform on the interruption of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandInterrupt(Consumer<Command> action) {
+    requireNonNullParam(action, "action", "onCommandInterrupt");
+    m_interruptActions.add((command, interruptor) -> action.accept(command));
+  }
+
+  /**
+   * Adds an action to perform on the interruption of any command by the scheduler. The action
+   * receives the interrupted command and an Optional containing the interrupting command, or
+   * Optional.empty() if it was not canceled by a command (e.g., by {@link
+   * CommandScheduler#cancel}).
+   *
+   * @param action the action to perform
+   */
+  public void onCommandInterrupt(BiConsumer<Command, Optional<Command>> action) {
+    m_interruptActions.add(requireNonNullParam(action, "action", "onCommandInterrupt"));
+  }
+
+  /**
+   * Adds an action to perform on the finishing of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  public void onCommandFinish(Consumer<Command> action) {
+    m_finishActions.add(requireNonNullParam(action, "action", "onCommandFinish"));
+  }
+
+  /**
+   * Register commands as composed. An exception will be thrown if these commands are scheduled
+   * directly or added to a composition.
+   *
+   * @param commands the commands to register
+   * @throws IllegalArgumentException if the given commands have already been composed, or the array
+   *     of commands has duplicates.
+   */
+  public void registerComposedCommands(Command... commands) {
+    Set<Command> commandSet;
+    try {
+      commandSet = Set.of(commands);
+    } catch (IllegalArgumentException e) {
+      throw new IllegalArgumentException(
+          "Cannot compose a command twice in the same composition! (Original exception: "
+              + e
+              + ")");
+    }
+    requireNotComposedOrScheduled(commandSet);
+    var exception = new Exception("Originally composed at:");
+    exception.fillInStackTrace();
+    for (var command : commands) {
+      m_composedCommands.put(command, exception);
+    }
+  }
+
+  /**
+   * Clears the list of composed commands, allowing all commands to be freely used again.
+   *
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
+   * this unless you fully understand what you are doing.
+   */
+  public void clearComposedCommands() {
+    m_composedCommands.clear();
+  }
+
+  /**
+   * Removes a single command from the list of composed commands, allowing it to be freely used
+   * again.
+   *
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
+   * this unless you fully understand what you are doing.
+   *
+   * @param command the command to remove from the list of grouped commands
+   */
+  public void removeComposedCommand(Command command) {
+    m_composedCommands.remove(command);
+  }
+
+  /**
+   * Strip additional leading stack trace elements that are in the framework package.
+   *
+   * @param stacktrace the original stacktrace
+   * @return the stacktrace stripped of leading elements so there is at max one leading element from
+   *     the edu.wpi.first.wpilibj2.command package.
+   */
+  private StackTraceElement[] stripFrameworkStackElements(StackTraceElement[] stacktrace) {
+    int i = stacktrace.length - 1;
+    for (; i > 0; i--) {
+      if (stacktrace[i].getClassName().startsWith("edu.wpi.first.wpilibj2.command.")) {
+        break;
+      }
+    }
+    return Arrays.copyOfRange(stacktrace, i, stacktrace.length);
+  }
+
+  /**
+   * Requires that the specified command hasn't already been added to a composition.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been composed.
+   */
+  public void requireNotComposed(Command... commands) {
+    for (var command : commands) {
+      var exception = m_composedCommands.getOrDefault(command, null);
+      if (exception != null) {
+        exception.setStackTrace(stripFrameworkStackElements(exception.getStackTrace()));
+        var buffer = new StringWriter();
+        var writer = new PrintWriter(buffer);
+        writer.println(
+            "Commands that have been composed may not be added to another composition or scheduled "
+                + "individually!");
+        exception.printStackTrace(writer);
+        var thrownException = new IllegalArgumentException(buffer.toString());
+        thrownException.setStackTrace(stripFrameworkStackElements(thrownException.getStackTrace()));
+        throw thrownException;
+      }
+    }
+  }
+
+  /**
+   * Requires that the specified commands have not already been added to a composition.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been composed.
+   */
+  public void requireNotComposed(Collection<Command> commands) {
+    requireNotComposed(commands.toArray(Command[]::new));
+  }
+
+  /**
+   * Requires that the specified command hasn't already been added to a composition, and is not
+   * currently scheduled.
+   *
+   * @param command The command to check
+   * @throws IllegalArgumentException if the given command has already been composed or scheduled.
+   */
+  public void requireNotComposedOrScheduled(Command command) {
+    if (isScheduled(command)) {
+      throw new IllegalArgumentException(
+          "Commands that have been scheduled individually may not be added to a composition!");
+    }
+    requireNotComposed(command);
+  }
+
+  /**
+   * Requires that the specified commands have not already been added to a composition, and are not
+   * currently scheduled.
+   *
+   * @param commands The commands to check
+   * @throws IllegalArgumentException if the given commands have already been composed or scheduled.
+   */
+  public void requireNotComposedOrScheduled(Collection<Command> commands) {
+    for (var command : commands) {
+      requireNotComposedOrScheduled(command);
+    }
+  }
+
+  /**
+   * Check if the given command has been composed.
+   *
+   * @param command The command to check
+   * @return true if composed
+   */
+  public boolean isComposed(Command command) {
+    return getComposedCommands().contains(command);
+  }
+
+  Set<Command> getComposedCommands() {
+    return m_composedCommands.keySet();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Scheduler");
+    builder.addStringArrayProperty(
+        "Names",
+        () -> {
+          String[] names = new String[m_scheduledCommands.size()];
+          int i = 0;
+          for (Command command : m_scheduledCommands) {
+            names[i] = command.getName();
+            i++;
+          }
+          return names;
+        },
+        null);
+    builder.addIntegerArrayProperty(
+        "Ids",
+        () -> {
+          long[] ids = new long[m_scheduledCommands.size()];
+          int i = 0;
+          for (Command command : m_scheduledCommands) {
+            ids[i] = command.hashCode();
+            i++;
+          }
+          return ids;
+        },
+        null);
+    builder.addIntegerArrayProperty(
+        "Cancel",
+        () -> new long[] {},
+        toCancel -> {
+          Map<Long, Command> ids = new LinkedHashMap<>();
+          for (Command command : m_scheduledCommands) {
+            long id = command.hashCode();
+            ids.put(id, command);
+          }
+          for (long hash : toCancel) {
+            cancel(ids.get(hash));
+          }
+        });
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/Commands.java

@@ -0,0 +1,274 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.units.measure.Time;
+import java.util.Map;
+import java.util.Set;
+import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
+
+/**
+ * Namespace for command factory methods.
+ *
+ * <p>For convenience, you might want to static import the members of this class.
+ */
+public final class Commands {
+  /**
+   * Constructs a command that does nothing, finishing immediately.
+   *
+   * @return the command
+   */
+  public static Command none() {
+    return new InstantCommand();
+  }
+
+  /**
+   * Constructs a command that does nothing until interrupted.
+   *
+   * @param requirements Subsystems to require
+   * @return the command
+   */
+  public static Command idle(Subsystem... requirements) {
+    return run(() -> {}, requirements);
+  }
+
+  // Action Commands
+
+  /**
+   * Constructs a command that runs an action once and finishes.
+   *
+   * @param action the action to run
+   * @param requirements subsystems the action requires
+   * @return the command
+   * @see InstantCommand
+   */
+  public static Command runOnce(Runnable action, Subsystem... requirements) {
+    return new InstantCommand(action, requirements);
+  }
+
+  /**
+   * Constructs a command that runs an action every iteration until interrupted.
+   *
+   * @param action the action to run
+   * @param requirements subsystems the action requires
+   * @return the command
+   * @see RunCommand
+   */
+  public static Command run(Runnable action, Subsystem... requirements) {
+    return new RunCommand(action, requirements);
+  }
+
+  /**
+   * Constructs a command that runs an action once and another action when the command is
+   * interrupted.
+   *
+   * @param start the action to run on start
+   * @param end the action to run on interrupt
+   * @param requirements subsystems the action requires
+   * @return the command
+   * @see StartEndCommand
+   */
+  public static Command startEnd(Runnable start, Runnable end, Subsystem... requirements) {
+    return new StartEndCommand(start, end, requirements);
+  }
+
+  /**
+   * Constructs a command that runs an action every iteration until interrupted, and then runs a
+   * second action.
+   *
+   * @param run the action to run every iteration
+   * @param end the action to run on interrupt
+   * @param requirements subsystems the action requires
+   * @return the command
+   */
+  public static Command runEnd(Runnable run, Runnable end, Subsystem... requirements) {
+    requireNonNullParam(end, "end", "Command.runEnd");
+    return new FunctionalCommand(
+        () -> {}, run, interrupted -> end.run(), () -> false, requirements);
+  }
+
+  /**
+   * Constructs a command that runs an action once, and then runs an action every iteration until
+   * interrupted.
+   *
+   * @param start the action to run on start
+   * @param run the action to run every iteration
+   * @param requirements subsystems the action requires
+   * @return the command
+   */
+  public static Command startRun(Runnable start, Runnable run, Subsystem... requirements) {
+    return new FunctionalCommand(start, run, interrupted -> {}, () -> false, requirements);
+  }
+
+  /**
+   * Constructs a command that prints a message and finishes.
+   *
+   * @param message the message to print
+   * @return the command
+   * @see PrintCommand
+   */
+  public static Command print(String message) {
+    return new PrintCommand(message);
+  }
+
+  // Idling Commands
+
+  /**
+   * Constructs a command that does nothing, finishing after a specified duration.
+   *
+   * @param time after how long the command finishes in seconds
+   * @return the command
+   * @see WaitCommand
+   */
+  public static Command wait(double time) {
+    return new WaitCommand(time);
+  }
+
+  /**
+   * Constructs a command that does nothing, finishing after a specified duration.
+   *
+   * @param time after how long the command finishes
+   * @return the command
+   * @see WaitCommand
+   */
+  public static Command waitTime(Time time) {
+    return new WaitCommand(time);
+  }
+
+  /**
+   * Constructs a command that does nothing, finishing once a condition becomes true.
+   *
+   * @param condition the condition
+   * @return the command
+   * @see WaitUntilCommand
+   */
+  public static Command waitUntil(BooleanSupplier condition) {
+    return new WaitUntilCommand(condition);
+  }
+
+  // Selector Commands
+
+  /**
+   * Runs one of two commands, based on the boolean selector function.
+   *
+   * @param onTrue the command to run if the selector function returns true
+   * @param onFalse the command to run if the selector function returns false
+   * @param selector the selector function
+   * @return the command
+   * @see ConditionalCommand
+   */
+  public static Command either(Command onTrue, Command onFalse, BooleanSupplier selector) {
+    return new ConditionalCommand(onTrue, onFalse, selector);
+  }
+
+  /**
+   * Runs one of several commands, based on the selector function.
+   *
+   * @param <K> The type of key used to select the command
+   * @param selector the selector function
+   * @param commands map of commands to select from
+   * @return the command
+   * @see SelectCommand
+   */
+  public static <K> Command select(Map<K, Command> commands, Supplier<? extends K> selector) {
+    return new SelectCommand<>(commands, selector);
+  }
+
+  /**
+   * Runs the command supplied by the supplier.
+   *
+   * @param supplier the command supplier
+   * @param requirements the set of requirements for this command
+   * @return the command
+   * @see DeferredCommand
+   */
+  public static Command defer(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    return new DeferredCommand(supplier, requirements);
+  }
+
+  /**
+   * Constructs a command that schedules the command returned from the supplier when initialized,
+   * and ends when it is no longer scheduled. The supplier is called when the command is
+   * initialized.
+   *
+   * @param supplier the command supplier
+   * @return the command
+   * @see ProxyCommand
+   * @see DeferredCommand
+   */
+  public static Command deferredProxy(Supplier<Command> supplier) {
+    return defer(() -> supplier.get().asProxy(), Set.of());
+  }
+
+  // Command Groups
+
+  /**
+   * Runs a group of commands in series, one after the other.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   * @see SequentialCommandGroup
+   */
+  public static Command sequence(Command... commands) {
+    return new SequentialCommandGroup(commands);
+  }
+
+  /**
+   * Runs a group of commands in series, one after the other. Once the last command ends, the group
+   * is restarted.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   * @see SequentialCommandGroup
+   * @see Command#repeatedly()
+   */
+  public static Command repeatingSequence(Command... commands) {
+    return sequence(commands).repeatedly();
+  }
+
+  /**
+   * Runs a group of commands at the same time. Ends once all commands in the group finish.
+   *
+   * @param commands the commands to include
+   * @return the command
+   * @see ParallelCommandGroup
+   */
+  public static Command parallel(Command... commands) {
+    return new ParallelCommandGroup(commands);
+  }
+
+  /**
+   * Runs a group of commands at the same time. Ends once any command in the group finishes, and
+   * cancels the others.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   * @see ParallelRaceGroup
+   */
+  public static Command race(Command... commands) {
+    return new ParallelRaceGroup(commands);
+  }
+
+  /**
+   * Runs a group of commands at the same time. Ends once a specific command finishes, and cancels
+   * the others.
+   *
+   * @param deadline the deadline command
+   * @param otherCommands the other commands to include
+   * @return the command group
+   * @see ParallelDeadlineGroup
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
+   */
+  public static Command deadline(Command deadline, Command... otherCommands) {
+    return new ParallelDeadlineGroup(deadline, otherCommands);
+  }
+
+  private Commands() {
+    throw new UnsupportedOperationException("This is a utility class");
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ConditionalCommand.java

@@ -0,0 +1,103 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command composition that runs one of two commands, depending on the value of the given
+ * condition when this command is initialized.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ConditionalCommand extends Command {
+  private final Command m_onTrue;
+  private final Command m_onFalse;
+  private final BooleanSupplier m_condition;
+  private Command m_selectedCommand;
+
+  /**
+   * Creates a new ConditionalCommand.
+   *
+   * @param onTrue the command to run if the condition is true
+   * @param onFalse the command to run if the condition is false
+   * @param condition the condition to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
+    m_onTrue = requireNonNullParam(onTrue, "onTrue", "ConditionalCommand");
+    m_onFalse = requireNonNullParam(onFalse, "onFalse", "ConditionalCommand");
+    m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand");
+
+    CommandScheduler.getInstance().registerComposedCommands(onTrue, onFalse);
+
+    addRequirements(m_onTrue.getRequirements());
+    addRequirements(m_onFalse.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if (m_condition.getAsBoolean()) {
+      m_selectedCommand = m_onTrue;
+    } else {
+      m_selectedCommand = m_onFalse;
+    }
+    m_selectedCommand.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_selectedCommand.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_selectedCommand.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_selectedCommand.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    if (m_onTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf
+        || m_onFalse.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+      return InterruptionBehavior.kCancelSelf;
+    } else {
+      return InterruptionBehavior.kCancelIncoming;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("onTrue", m_onTrue::getName, null);
+    builder.addStringProperty("onFalse", m_onFalse::getName, null);
+    builder.addStringProperty(
+        "selected",
+        () -> {
+          if (m_selectedCommand == null) {
+            return "null";
+          } else {
+            return m_selectedCommand.getName();
+          }
+        },
+        null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/DeferredCommand.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.Set;
+import java.util.function.Supplier;
+
+/**
+ * Defers Command construction to runtime. Runs the command returned by a supplier when this command
+ * is initialized, and ends when it ends. Useful for performing runtime tasks before creating a new
+ * command. If this command is interrupted, it will cancel the command.
+ *
+ * <p>Note that the supplier <i>must</i> create a new Command each call. For selecting one of a
+ * preallocated set of commands, use {@link SelectCommand}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class DeferredCommand extends Command {
+  private final Command m_nullCommand =
+      new PrintCommand("[DeferredCommand] Supplied command was null!");
+
+  private final Supplier<Command> m_supplier;
+  private Command m_command = m_nullCommand;
+
+  /**
+   * Creates a new DeferredCommand that directly runs the supplied command when initialized, and
+   * ends when it ends. Useful for lazily creating commands when the DeferredCommand is initialized,
+   * such as if the supplied command depends on runtime state. The {@link Supplier} will be called
+   * each time this command is initialized. The Supplier <i>must</i> create a new Command each call.
+   *
+   * @param supplier The command supplier
+   * @param requirements The command requirements. This is a {@link Set} to prevent accidental
+   *     omission of command requirements. Use {@link Set#of()} to easily construct a requirement
+   *     set.
+   */
+  @SuppressWarnings("this-escape")
+  public DeferredCommand(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    m_supplier = requireNonNullParam(supplier, "supplier", "DeferredCommand");
+    addRequirements(requirements.toArray(new Subsystem[0]));
+  }
+
+  @Override
+  public void initialize() {
+    var cmd = m_supplier.get();
+    if (cmd != null) {
+      m_command = cmd;
+      CommandScheduler.getInstance().registerComposedCommands(m_command);
+    }
+    m_command.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_command.isFinished();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+    m_command = m_nullCommand;
+  }
+
+  @Override
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty(
+        "deferred", () -> m_command == m_nullCommand ? "null" : m_command.getName(), null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/FunctionalCommand.java

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+/**
+ * A command that allows the user to pass in functions for each of the basic command methods through
+ * the constructor. Useful for inline definitions of complex commands - note, however, that if a
+ * command is beyond a certain complexity it is usually better practice to write a proper class for
+ * it than to inline it.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class FunctionalCommand extends Command {
+  private final Runnable m_onInit;
+  private final Runnable m_onExecute;
+  private final Consumer<Boolean> m_onEnd;
+  private final BooleanSupplier m_isFinished;
+
+  /**
+   * Creates a new FunctionalCommand.
+   *
+   * @param onInit the function to run on command initialization
+   * @param onExecute the function to run on command execution
+   * @param onEnd the function to run on command end
+   * @param isFinished the function that determines whether the command has finished
+   * @param requirements the subsystems required by this command
+   */
+  @SuppressWarnings("this-escape")
+  public FunctionalCommand(
+      Runnable onInit,
+      Runnable onExecute,
+      Consumer<Boolean> onEnd,
+      BooleanSupplier isFinished,
+      Subsystem... requirements) {
+    m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
+    m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
+    m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
+    m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand");
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_onInit.run();
+  }
+
+  @Override
+  public void execute() {
+    m_onExecute.run();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_onEnd.accept(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_isFinished.getAsBoolean();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/InstantCommand.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on the same iteration of
+ * the scheduler. Users can either pass in a Runnable and a set of requirements, or else subclass
+ * this command if desired.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class InstantCommand extends FunctionalCommand {
+  /**
+   * Creates a new InstantCommand that runs the given Runnable with the given requirements.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the subsystems required by this command
+   */
+  public InstantCommand(Runnable toRun, Subsystem... requirements) {
+    super(toRun, () -> {}, interrupted -> {}, () -> true, requirements);
+  }
+
+  /**
+   * Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
+   * constructor to call implicitly from subclass constructors.
+   */
+  public InstantCommand() {
+    this(() -> {});
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/NotifierCommand.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Notifier;
+
+/**
+ * A command that starts a notifier to run the given runnable periodically in a separate thread. Has
+ * no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or {@link
+ * Command#until(java.util.function.BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to make the executed
+ * code thread-safe. If you do not know what "thread-safe" means, that is a good sign that you
+ * should not use this class.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class NotifierCommand extends Command {
+  private final Notifier m_notifier;
+  private final double m_period;
+
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun the runnable for the notifier to run
+   * @param period the period at which the notifier should run, in seconds
+   * @param requirements the subsystems required by this command
+   */
+  @SuppressWarnings("this-escape")
+  public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
+    m_notifier = new Notifier(toRun);
+    m_period = period;
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_notifier.startPeriodic(m_period);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_notifier.stop();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ParallelCommandGroup.java

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.LinkedHashMap;
+import java.util.Map;
+
+/**
+ * A command composition that runs a set of commands in parallel, ending when the last command ends.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ParallelCommandGroup extends Command {
+  // maps commands in this composition to whether they are still running
+  // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that
+  // changing the value associated with a command does NOT change the order)
+  private final Map<Command, Boolean> m_commands = new LinkedHashMap<>();
+  private boolean m_runWhenDisabled = true;
+  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
+
+  /**
+   * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
+   * command composition will finish when the last command finishes. If the composition is
+   * interrupted, only the commands that are still running will be interrupted.
+   *
+   * @param commands the commands to include in this composition.
+   */
+  @SuppressWarnings("this-escape")
+  public ParallelCommandGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  public final void addCommands(Command... commands) {
+    if (m_commands.containsValue(true)) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a composition while it's running");
+    }
+
+    CommandScheduler.getInstance().registerComposedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel composition cannot require the same subsystems");
+      }
+      m_commands.put(command, false);
+      addRequirements(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+      }
+    }
+  }
+
+  @Override
+  public final void initialize() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      commandRunning.getKey().initialize();
+      commandRunning.setValue(true);
+    }
+  }
+
+  @Override
+  public final void execute() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (!commandRunning.getValue()) {
+        continue;
+      }
+      commandRunning.getKey().execute();
+      if (commandRunning.getKey().isFinished()) {
+        commandRunning.getKey().end(false);
+        commandRunning.setValue(false);
+      }
+    }
+  }
+
+  @Override
+  public final void end(boolean interrupted) {
+    if (interrupted) {
+      for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+        if (commandRunning.getValue()) {
+          commandRunning.getKey().end(true);
+        }
+      }
+    }
+  }
+
+  @Override
+  public final boolean isFinished() {
+    return !m_commands.containsValue(true);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_interruptBehavior;
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ParallelDeadlineGroup.java

@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.Collections;
+import java.util.LinkedHashMap;
+import java.util.Map;
+
+/**
+ * A command composition that runs a set of commands in parallel, ending only when a specific
+ * command (the "deadline") ends, interrupting all other commands that are still running at that
+ * point.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ParallelDeadlineGroup extends Command {
+  // maps commands in this composition to whether they are still running
+  // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that
+  // changing the value associated with a command does NOT change the order)
+  private final Map<Command, Boolean> m_commands = new LinkedHashMap<>();
+  private boolean m_runWhenDisabled = true;
+  private boolean m_finished = true;
+  private Command m_deadline;
+  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
+
+  /**
+   * Creates a new ParallelDeadlineGroup. The given commands, including the deadline, will be
+   * executed simultaneously. The composition will finish when the deadline finishes, interrupting
+   * all other still-running commands. If the composition is interrupted, only the commands still
+   * running will be interrupted.
+   *
+   * @param deadline the command that determines when the composition ends
+   * @param otherCommands the other commands to be executed
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
+   */
+  @SuppressWarnings("this-escape")
+  public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
+    setDeadline(deadline);
+    addCommands(otherCommands);
+  }
+
+  /**
+   * Sets the deadline to the given command. The deadline is added to the group if it is not already
+   * contained.
+   *
+   * @param deadline the command that determines when the group ends
+   * @throws IllegalArgumentException if the deadline command is already in the composition
+   */
+  public final void setDeadline(Command deadline) {
+    @SuppressWarnings("PMD.CompareObjectsWithEquals")
+    boolean isAlreadyDeadline = deadline == m_deadline;
+    if (isAlreadyDeadline) {
+      return;
+    }
+    if (m_commands.containsKey(deadline)) {
+      throw new IllegalArgumentException(
+          "The deadline command cannot also be in the other commands!");
+    }
+    addCommands(deadline);
+    m_deadline = deadline;
+  }
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  public final void addCommands(Command... commands) {
+    if (!m_finished) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a composition while it's running");
+    }
+
+    CommandScheduler.getInstance().registerComposedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel group cannot require the same subsystems");
+      }
+      m_commands.put(command, false);
+      addRequirements(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+      }
+    }
+  }
+
+  @Override
+  public final void initialize() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      commandRunning.getKey().initialize();
+      commandRunning.setValue(true);
+    }
+    m_finished = false;
+  }
+
+  @Override
+  public final void execute() {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (!commandRunning.getValue()) {
+        continue;
+      }
+      commandRunning.getKey().execute();
+      if (commandRunning.getKey().isFinished()) {
+        commandRunning.getKey().end(false);
+        commandRunning.setValue(false);
+        if (commandRunning.getKey().equals(m_deadline)) {
+          m_finished = true;
+        }
+      }
+    }
+  }
+
+  @Override
+  public final void end(boolean interrupted) {
+    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+      if (commandRunning.getValue()) {
+        commandRunning.getKey().end(true);
+      }
+    }
+  }
+
+  @Override
+  public final boolean isFinished() {
+    return m_finished;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_interruptBehavior;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+
+    builder.addStringProperty("deadline", m_deadline::getName, null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ParallelRaceGroup.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.LinkedHashSet;
+import java.util.Set;
+
+/**
+ * A composition that runs a set of commands in parallel, ending when any one of the commands ends
+ * and interrupting all the others.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ParallelRaceGroup extends Command {
+  // LinkedHashSet guarantees we iterate over commands in the order they were added
+  private final Set<Command> m_commands = new LinkedHashSet<>();
+  private boolean m_runWhenDisabled = true;
+  private boolean m_finished = true;
+  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
+
+  /**
+   * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will
+   * "race to the finish" - the first command to finish ends the entire command, with all other
+   * commands being interrupted.
+   *
+   * @param commands the commands to include in this composition.
+   */
+  @SuppressWarnings("this-escape")
+  public ParallelRaceGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  public final void addCommands(Command... commands) {
+    if (!m_finished) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a composition while it's running!");
+    }
+
+    CommandScheduler.getInstance().registerComposedCommands(commands);
+
+    for (Command command : commands) {
+      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel composition cannot require the same subsystems");
+      }
+      m_commands.add(command);
+      addRequirements(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+      }
+    }
+  }
+
+  @Override
+  public final void initialize() {
+    m_finished = false;
+    for (Command command : m_commands) {
+      command.initialize();
+    }
+  }
+
+  @Override
+  public final void execute() {
+    for (Command command : m_commands) {
+      command.execute();
+      if (command.isFinished()) {
+        m_finished = true;
+      }
+    }
+  }
+
+  @Override
+  public final void end(boolean interrupted) {
+    for (Command command : m_commands) {
+      command.end(!command.isFinished());
+    }
+  }
+
+  @Override
+  public final boolean isFinished() {
+    return m_finished;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_interruptBehavior;
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/PrintCommand.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+/**
+ * A command that prints a string when initialized.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class PrintCommand extends InstantCommand {
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  public PrintCommand(String message) {
+    super(() -> System.out.println(message));
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ProxyCommand.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.function.Supplier;
+
+/**
+ * Schedules a given command when this command is initialized and ends when it ends, but does not
+ * directly run it. Use this for including a command in a composition without adding its
+ * requirements, <strong>but only if you know what you are doing. If you are unsure, see <a
+ * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+ * WPILib docs</a> for a complete explanation of proxy semantics.</strong> Do not proxy a command
+ * from a subsystem already required by the composition, or else the composition will cancel itself
+ * when the proxy is reached. If this command is interrupted, it will cancel the command.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ProxyCommand extends Command {
+  private final Supplier<Command> m_supplier;
+  private Command m_command;
+
+  /**
+   * Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when
+   * it is no longer scheduled. Use this for lazily creating <strong>proxied</strong> commands at
+   * runtime. Proxying should only be done to escape from composition requirement semantics, so if
+   * only initialization time command construction is needed, use {@link DeferredCommand} instead.
+   *
+   * @param supplier the command supplier
+   * @deprecated This constructor's similarity to {@link DeferredCommand} is confusing and opens
+   *     potential footguns for users who do not fully understand the semantics and implications of
+   *     proxying, but who simply want runtime construction. Users who do know what they are doing
+   *     and need a supplier-constructed proxied command should instead defer a proxy command.
+   * @see DeferredCommand
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public ProxyCommand(Supplier<Command> supplier) {
+    m_supplier = requireNonNullParam(supplier, "supplier", "ProxyCommand");
+  }
+
+  /**
+   * Creates a new ProxyCommand that schedules the given command when initialized, and ends when it
+   * is no longer scheduled.
+   *
+   * @param command the command to run by proxy
+   */
+  @SuppressWarnings("this-escape")
+  public ProxyCommand(Command command) {
+    Command nullCheckedCommand = requireNonNullParam(command, "command", "ProxyCommand");
+    m_supplier = () -> nullCheckedCommand;
+    setName("Proxy(" + nullCheckedCommand.getName() + ")");
+  }
+
+  @Override
+  public void initialize() {
+    m_command = m_supplier.get();
+    CommandScheduler.getInstance().schedule(m_command);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) {
+      m_command.cancel();
+    }
+    m_command = null;
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public boolean isFinished() {
+    // because we're between `initialize` and `end`, `m_command` is necessarily not null
+    // but if called otherwise and m_command is null,
+    // it's UB, so we can do whatever we want -- like return true.
+    return m_command == null || !m_command.isScheduled();
+  }
+
+  /**
+   * Whether the given command should run when the robot is disabled. Override to return true if the
+   * command should run when disabled.
+   *
+   * @return true. Otherwise, this proxy would cancel commands that do run when disabled.
+   */
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty(
+        "proxied", () -> m_command == null ? "null" : m_command.getName(), null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/RepeatCommand.java

@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+/**
+ * A command that runs another command repeatedly, restarting it when it ends, until this command is
+ * interrupted. Command instances that are passed to it cannot be added to any other groups, or
+ * scheduled individually.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually,and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class RepeatCommand extends Command {
+  private final Command m_command;
+  private boolean m_ended;
+
+  /**
+   * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it
+   * ends, until this command is interrupted.
+   *
+   * @param command the command to run repeatedly
+   */
+  @SuppressWarnings("this-escape")
+  public RepeatCommand(Command command) {
+    m_command = requireNonNullParam(command, "command", "RepeatCommand");
+    CommandScheduler.getInstance().registerComposedCommands(command);
+    addRequirements(command.getRequirements());
+    setName("Repeat(" + command.getName() + ")");
+  }
+
+  @Override
+  public void initialize() {
+    m_ended = false;
+    m_command.initialize();
+  }
+
+  @Override
+  public void execute() {
+    if (m_ended) {
+      m_ended = false;
+      m_command.initialize();
+    }
+    m_command.execute();
+    if (m_command.isFinished()) {
+      // restart command
+      m_command.end(false);
+      m_ended = true;
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    // Make sure we didn't already call end() (which would happen if the command finished in the
+    // last call to our execute())
+    if (!m_ended) {
+      m_command.end(interrupted);
+      m_ended = true;
+    }
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_command.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_command.getInterruptionBehavior();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("command", m_command::getName, null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/RunCommand.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or
+ * use {@link Command#withTimeout(double)} or {@link Command#until(BooleanSupplier)} to give it one.
+ * If you only wish to execute a Runnable once, use {@link InstantCommand}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class RunCommand extends FunctionalCommand {
+  /**
+   * Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does
+   * not run when disabled.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the subsystems to require
+   */
+  public RunCommand(Runnable toRun, Subsystem... requirements) {
+    super(() -> {}, toRun, interrupted -> {}, () -> false, requirements);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/ScheduleCommand.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * Schedules the given commands when this command is initialized. Useful for forking off from
+ * CommandGroups. Note that if run from a composition, the composition will not know about the
+ * status of the scheduled commands, and will treat this command as finishing instantly.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class ScheduleCommand extends Command {
+  private final Set<Command> m_toSchedule;
+
+  /**
+   * Creates a new ScheduleCommand that schedules the given commands when initialized.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  public ScheduleCommand(Command... toSchedule) {
+    m_toSchedule = Set.of(toSchedule);
+  }
+
+  @Override
+  public void initialize() {
+    for (Command command : m_toSchedule) {
+      CommandScheduler.getInstance().schedule(command);
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/SelectCommand.java

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.Map;
+import java.util.function.Supplier;
+
+/**
+ * A command composition that runs one of a selection of commands using a selector and a key to
+ * command mapping.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ *
+ * @param <K> The type of key used to select the command
+ */
+public class SelectCommand<K> extends Command {
+  private final Map<K, Command> m_commands;
+  private final Supplier<? extends K> m_selector;
+  private Command m_selectedCommand;
+  private boolean m_runsWhenDisabled = true;
+  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
+
+  private final Command m_defaultCommand =
+      new PrintCommand("SelectCommand selector value does not correspond to any command!");
+
+  /**
+   * Creates a new SelectCommand.
+   *
+   * @param commands the map of commands to choose from
+   * @param selector the selector to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public SelectCommand(Map<K, Command> commands, Supplier<? extends K> selector) {
+    m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
+    m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
+
+    CommandScheduler.getInstance().registerComposedCommands(m_defaultCommand);
+    CommandScheduler.getInstance()
+        .registerComposedCommands(commands.values().toArray(new Command[] {}));
+
+    for (Command command : m_commands.values()) {
+      addRequirements(command.getRequirements());
+      m_runsWhenDisabled &= command.runsWhenDisabled();
+      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+      }
+    }
+  }
+
+  @Override
+  public void initialize() {
+    m_selectedCommand = m_commands.getOrDefault(m_selector.get(), m_defaultCommand);
+    m_selectedCommand.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_selectedCommand.execute();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_selectedCommand.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_selectedCommand.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runsWhenDisabled;
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_interruptBehavior;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+
+    builder.addStringProperty(
+        "selected", () -> m_selectedCommand == null ? "null" : m_selectedCommand.getName(), null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/SequentialCommandGroup.java

@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * A command composition that runs a list of commands in sequence.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class SequentialCommandGroup extends Command {
+  private final List<Command> m_commands = new ArrayList<>();
+  private int m_currentCommandIndex = -1;
+  private boolean m_runWhenDisabled = true;
+  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
+
+  /**
+   * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
+   * composition finishing when the last command finishes.
+   *
+   * @param commands the commands to include in this composition.
+   */
+  @SuppressWarnings("this-escape")
+  public SequentialCommandGroup(Command... commands) {
+    addCommands(commands);
+  }
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add, in order of execution.
+   */
+  @SuppressWarnings("PMD.UseArraysAsList")
+  public final void addCommands(Command... commands) {
+    if (m_currentCommandIndex != -1) {
+      throw new IllegalStateException(
+          "Commands cannot be added to a composition while it's running");
+    }
+
+    CommandScheduler.getInstance().registerComposedCommands(commands);
+
+    for (Command command : commands) {
+      m_commands.add(command);
+      addRequirements(command.getRequirements());
+      m_runWhenDisabled &= command.runsWhenDisabled();
+      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+      }
+    }
+  }
+
+  @Override
+  public final void initialize() {
+    m_currentCommandIndex = 0;
+
+    if (!m_commands.isEmpty()) {
+      m_commands.get(0).initialize();
+    }
+  }
+
+  @Override
+  public final void execute() {
+    if (m_commands.isEmpty()) {
+      return;
+    }
+
+    Command currentCommand = m_commands.get(m_currentCommandIndex);
+
+    currentCommand.execute();
+    if (currentCommand.isFinished()) {
+      currentCommand.end(false);
+      m_currentCommandIndex++;
+      if (m_currentCommandIndex < m_commands.size()) {
+        m_commands.get(m_currentCommandIndex).initialize();
+      }
+    }
+  }
+
+  @Override
+  public final void end(boolean interrupted) {
+    if (interrupted
+        && !m_commands.isEmpty()
+        && m_currentCommandIndex > -1
+        && m_currentCommandIndex < m_commands.size()) {
+      m_commands.get(m_currentCommandIndex).end(true);
+    }
+    m_currentCommandIndex = -1;
+  }
+
+  @Override
+  public final boolean isFinished() {
+    return m_currentCommandIndex == m_commands.size();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_runWhenDisabled;
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_interruptBehavior;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+
+    builder.addIntegerProperty("index", () -> m_currentCommandIndex, null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/StartEndCommand.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import java.util.function.Consumer;
+
+/**
+ * A command that runs a given runnable when it is initialized, and another runnable when it ends.
+ * Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no
+ * end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or {@link
+ * Command#until(java.util.function.BooleanSupplier)} to give it one.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class StartEndCommand extends FunctionalCommand {
+  /**
+   * Creates a new StartEndCommand. Will run the given runnables when the command starts and when it
+   * ends.
+   *
+   * @param onInit the Runnable to run on command init
+   * @param onEnd the Runnable to run on command end
+   * @param requirements the subsystems required by this command
+   */
+  public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
+    super(
+        onInit,
+        () -> {},
+        // we need to do some magic here to null-check `onEnd` before it's captured
+        droppingParameter(requireNonNullParam(onEnd, "onEnd", "StartEndCommand")),
+        () -> false,
+        requirements);
+  }
+
+  private static Consumer<Boolean> droppingParameter(Runnable run) {
+    return bool -> run.run();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/Subsystem.java

@@ -0,0 +1,181 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.Supplier;
+
+/**
+ * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
+ * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
+ * provide methods through which they can be used by {@link Command}s. Subsystems are used by the
+ * {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not
+ * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in
+ * their {@link Command#getRequirements()} method, and resources used within a subsystem should
+ * generally remain encapsulated and not be shared by other parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the {@link
+ * CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link
+ * Subsystem#periodic()} method to be called. It is recommended that this method be called from the
+ * constructor of users' Subsystem implementations. The {@link SubsystemBase} class offers a simple
+ * base for user implementations that handles this.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public interface Subsystem {
+  /**
+   * This method is called periodically by the {@link CommandScheduler}. Useful for updating
+   * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should try
+   * to be consistent within their own codebases about which responsibilities will be handled by
+   * Commands, and which will be handled here.
+   */
+  default void periodic() {}
+
+  /**
+   * This method is called periodically by the {@link CommandScheduler}. Useful for updating
+   * subsystem-specific state that needs to be maintained for simulations, such as for updating
+   * {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
+   */
+  default void simulationPeriodic() {}
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  default String getName() {
+    return this.getClass().getSimpleName();
+  }
+
+  /**
+   * Sets the default {@link Command} of the subsystem. The default command will be automatically
+   * scheduled when no other commands are scheduled that require the subsystem. Default commands
+   * should generally not end on their own, i.e. their {@link Command#isFinished()} method should
+   * always return false. Will automatically register this subsystem with the {@link
+   * CommandScheduler}.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  default void setDefaultCommand(Command defaultCommand) {
+    CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
+  }
+
+  /**
+   * Removes the default command for the subsystem. This will not cancel the default command if it
+   * is currently running.
+   */
+  default void removeDefaultCommand() {
+    CommandScheduler.getInstance().removeDefaultCommand(this);
+  }
+
+  /**
+   * Gets the default command for this subsystem. Returns null if no default command is currently
+   * associated with the subsystem.
+   *
+   * @return the default command associated with this subsystem
+   */
+  default Command getDefaultCommand() {
+    return CommandScheduler.getInstance().getDefaultCommand(this);
+  }
+
+  /**
+   * Returns the command currently running on this subsystem. Returns null if no command is
+   * currently scheduled that requires this subsystem.
+   *
+   * @return the scheduled command currently requiring this subsystem
+   */
+  default Command getCurrentCommand() {
+    return CommandScheduler.getInstance().requiring(this);
+  }
+
+  /**
+   * Registers this subsystem with the {@link CommandScheduler}, allowing its {@link
+   * Subsystem#periodic()} method to be called when the scheduler runs.
+   */
+  default void register() {
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  /**
+   * Constructs a command that does nothing until interrupted. Requires this subsystem.
+   *
+   * @return the command
+   */
+  default Command idle() {
+    return Commands.idle(this);
+  }
+
+  /**
+   * Constructs a command that runs an action once and finishes. Requires this subsystem.
+   *
+   * @param action the action to run
+   * @return the command
+   * @see InstantCommand
+   */
+  default Command runOnce(Runnable action) {
+    return Commands.runOnce(action, this);
+  }
+
+  /**
+   * Constructs a command that runs an action every iteration until interrupted. Requires this
+   * subsystem.
+   *
+   * @param action the action to run
+   * @return the command
+   * @see RunCommand
+   */
+  default Command run(Runnable action) {
+    return Commands.run(action, this);
+  }
+
+  /**
+   * Constructs a command that runs an action once and another action when the command is
+   * interrupted. Requires this subsystem.
+   *
+   * @param start the action to run on start
+   * @param end the action to run on interrupt
+   * @return the command
+   * @see StartEndCommand
+   */
+  default Command startEnd(Runnable start, Runnable end) {
+    return Commands.startEnd(start, end, this);
+  }
+
+  /**
+   * Constructs a command that runs an action every iteration until interrupted, and then runs a
+   * second action. Requires this subsystem.
+   *
+   * @param run the action to run every iteration
+   * @param end the action to run on interrupt
+   * @return the command
+   */
+  default Command runEnd(Runnable run, Runnable end) {
+    return Commands.runEnd(run, end, this);
+  }
+
+  /**
+   * Constructs a command that runs an action once and then runs another action every iteration
+   * until interrupted. Requires this subsystem.
+   *
+   * @param start the action to run on start
+   * @param run the action to run every iteration
+   * @return the command
+   */
+  default Command startRun(Runnable start, Runnable run) {
+    return Commands.startRun(start, run, this);
+  }
+
+  /**
+   * Constructs a {@link DeferredCommand} with the provided supplier. This subsystem is added as a
+   * requirement.
+   *
+   * @param supplier the command supplier.
+   * @return the command.
+   * @see DeferredCommand
+   */
+  default Command defer(Supplier<Command> supplier) {
+    return Commands.defer(supplier, Set.of(this));
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/SubsystemBase.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * A base for subsystems that handles registration in the constructor, and provides a more intuitive
+ * method for setting the default command.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public abstract class SubsystemBase implements Subsystem, Sendable {
+  /** Constructor. Telemetry/log name defaults to the classname. */
+  @SuppressWarnings("this-escape")
+  public SubsystemBase() {
+    String name = this.getClass().getSimpleName();
+    name = name.substring(name.lastIndexOf('.') + 1);
+    SendableRegistry.add(this, name, name);
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  @SuppressWarnings("this-escape")
+  public SubsystemBase(String name) {
+    SendableRegistry.add(this, name, name);
+    CommandScheduler.getInstance().registerSubsystem(this);
+  }
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  @Override
+  public String getName() {
+    return SendableRegistry.getName(this);
+  }
+
+  /**
+   * Sets the name of this Subsystem.
+   *
+   * @param name name
+   */
+  public void setName(String name) {
+    SendableRegistry.setName(this, name);
+  }
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  public String getSubsystem() {
+    return SendableRegistry.getSubsystem(this);
+  }
+
+  /**
+   * Sets the subsystem name of this Subsystem.
+   *
+   * @param subsystem subsystem name
+   */
+  public void setSubsystem(String subsystem) {
+    SendableRegistry.setSubsystem(this, subsystem);
+  }
+
+  /**
+   * Associates a {@link Sendable} with this Subsystem. Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  public void addChild(String name, Sendable child) {
+    SendableRegistry.add(child, getSubsystem(), name);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Subsystem");
+
+    builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
+    builder.addStringProperty(
+        ".default",
+        () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none",
+        null);
+    builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
+    builder.addStringProperty(
+        ".command",
+        () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none",
+        null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/WaitCommand.java

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * A command that does nothing but takes a specified amount of time to finish.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WaitCommand extends Command {
+  /** The timer used for waiting. */
+  protected Timer m_timer = new Timer();
+
+  private final double m_duration;
+
+  /**
+   * Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
+   *
+   * @param seconds the time to wait, in seconds
+   */
+  @SuppressWarnings("this-escape")
+  public WaitCommand(double seconds) {
+    m_duration = seconds;
+    SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
+  }
+
+  /**
+   * Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
+   *
+   * @param time the time to wait
+   */
+  public WaitCommand(Time time) {
+    this(time.in(Seconds));
+  }
+
+  @Override
+  public void initialize() {
+    m_timer.restart();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasElapsed(m_duration);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addDoubleProperty("duration", () -> m_duration, null);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/WaitUntilCommand.java

@@ -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 edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.wpilibj.Timer;
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command that does nothing but ends after a specified match time or condition. Useful for
+ * CommandGroups.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WaitUntilCommand extends Command {
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WaitUntilCommand that ends after a given condition becomes true.
+   *
+   * @param condition the condition to determine when to end
+   */
+  public WaitUntilCommand(BooleanSupplier condition) {
+    m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand");
+  }
+
+  /**
+   * Creates a new WaitUntilCommand that ends after a given match time.
+   *
+   * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT
+   * guarantee that the time at which the action is performed will be judged to be legal by the
+   * referees. When in doubt, add a safety factor or time the action manually.
+   *
+   * @param time the match time after which to end, in seconds
+   */
+  public WaitUntilCommand(double time) {
+    this(() -> Timer.getMatchTime() - time > 0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_condition.getAsBoolean();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/WrapperCommand.java

@@ -0,0 +1,103 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * A class used internally to wrap commands while overriding a specific method; all other methods
+ * will call through to the wrapped command.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ */
+public abstract class WrapperCommand extends Command {
+  /** Command being wrapped. */
+  protected final Command m_command;
+
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this command or add it to
+   *     a composition will throw an exception.
+   */
+  @SuppressWarnings("this-escape")
+  protected WrapperCommand(Command command) {
+    CommandScheduler.getInstance().registerComposedCommands(command);
+    m_command = command;
+    // copy the wrapped command's name
+    setName(command.getName());
+  }
+
+  /** The initial subroutine of a command. Called once when the command is initially scheduled. */
+  @Override
+  public void initialize() {
+    m_command.initialize();
+  }
+
+  /** The main body of a command. Called repeatedly while the command is scheduled. */
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  /**
+   * The action to take when the command ends. Called when either the command finishes normally, or
+   * when it interrupted/canceled.
+   *
+   * <p>Do not schedule commands here that share requirements with this command. Use {@link
+   * #andThen(Command...)} instead.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+  }
+
+  /**
+   * Whether the command has finished. Once a command finishes, the scheduler will call its end()
+   * method and un-schedule it.
+   *
+   * @return whether the command has finished.
+   */
+  @Override
+  public boolean isFinished() {
+    return m_command.isFinished();
+  }
+
+  /**
+   * Specifies the set of subsystems used by this command. Two commands cannot use the same
+   * subsystem at the same time. If the command is scheduled as interruptible and another command is
+   * scheduled that shares a requirement, the command will be interrupted. Else, the command will
+   * not be scheduled. If no subsystems are required, return an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the requirements as a field, and
+   * return that field here, rather than allocating a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   */
+  @Override
+  public Set<Subsystem> getRequirements() {
+    return m_command.getRequirements();
+  }
+
+  /**
+   * Whether the given command should run when the robot is disabled. Override to return true if the
+   * command should run when disabled.
+   *
+   * @return whether the command should run when the robot is disabled
+   */
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_command.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    return m_command.getInterruptionBehavior();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/CommandGamepad.java

@@ -0,0 +1,745 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.Gamepad;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * A version of {@link Gamepad} with {@link Trigger} factories for command-based.
+ *
+ * @see Gamepad
+ */
+@SuppressWarnings("MethodName")
+public class CommandGamepad extends CommandGenericHID {
+  private final Gamepad m_hid;
+
+  /**
+   * Construct an instance of a controller.
+   *
+   * @param port The port index on the Driver Station that the controller is plugged into.
+   */
+  public CommandGamepad(int port) {
+    super(port);
+    m_hid = new Gamepad(port);
+  }
+
+  /**
+   * Get the underlying GenericHID object.
+   *
+   * @return the wrapped GenericHID object
+   */
+  @Override
+  public Gamepad getHID() {
+    return m_hid;
+  }
+
+  /**
+   * Constructs a Trigger instance around the South Face button's digital signal.
+   *
+   * @return a Trigger instance representing the South Face button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #southFace(EventLoop)
+   */
+  public Trigger southFace() {
+    return southFace(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the South Face button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the South Face button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger southFace(EventLoop loop) {
+    return button(Gamepad.Button.kSouthFace.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the East Face button's digital signal.
+   *
+   * @return a Trigger instance representing the East Face button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #eastFace(EventLoop)
+   */
+  public Trigger eastFace() {
+    return eastFace(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the East Face button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the East Face button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger eastFace(EventLoop loop) {
+    return button(Gamepad.Button.kEastFace.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the West Face button's digital signal.
+   *
+   * @return a Trigger instance representing the West Face button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #westFace(EventLoop)
+   */
+  public Trigger westFace() {
+    return westFace(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the West Face button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the West Face button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger westFace(EventLoop loop) {
+    return button(Gamepad.Button.kWestFace.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the North Face button's digital signal.
+   *
+   * @return a Trigger instance representing the North Face button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #northFace(EventLoop)
+   */
+  public Trigger northFace() {
+    return northFace(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the North Face button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the North Face button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger northFace(EventLoop loop) {
+    return button(Gamepad.Button.kNorthFace.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Back button's digital signal.
+   *
+   * @return a Trigger instance representing the Back button's digital signal attached to the {@link
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #back(EventLoop)
+   */
+  public Trigger back() {
+    return back(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Back button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Back button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger back(EventLoop loop) {
+    return button(Gamepad.Button.kBack.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Guide button's digital signal.
+   *
+   * @return a Trigger instance representing the Guide button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #guide(EventLoop)
+   */
+  public Trigger guide() {
+    return guide(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Guide button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Guide button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger guide(EventLoop loop) {
+    return button(Gamepad.Button.kGuide.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Start button's digital signal.
+   *
+   * @return a Trigger instance representing the Start button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #start(EventLoop)
+   */
+  public Trigger start() {
+    return start(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Start button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Start button's digital signal attached to the given
+   *     loop.
+   */
+  public Trigger start(EventLoop loop) {
+    return button(Gamepad.Button.kStart.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the left stick button's digital signal.
+   *
+   * @return a Trigger instance representing the left stick button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftStick(EventLoop)
+   */
+  public Trigger leftStick() {
+    return leftStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the left stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the left stick button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger leftStick(EventLoop loop) {
+    return button(Gamepad.Button.kLeftStick.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the right stick button's digital signal.
+   *
+   * @return a Trigger instance representing the right stick button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightStick(EventLoop)
+   */
+  public Trigger rightStick() {
+    return rightStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the right stick button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the right stick button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger rightStick(EventLoop loop) {
+    return button(Gamepad.Button.kRightStick.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's digital signal.
+   *
+   * @return a Trigger instance representing the right shoulder button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftShoulder(EventLoop)
+   */
+  public Trigger leftShoulder() {
+    return leftShoulder(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the right shoulder button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger leftShoulder(EventLoop loop) {
+    return button(Gamepad.Button.kLeftShoulder.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's digital signal.
+   *
+   * @return a Trigger instance representing the right shoulder button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightShoulder(EventLoop)
+   */
+  public Trigger rightShoulder() {
+    return rightShoulder(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the right shoulder button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger rightShoulder(EventLoop loop) {
+    return button(Gamepad.Button.kRightShoulder.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad up button's digital signal.
+   *
+   * @return a Trigger instance representing the D-pad up button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #dpadUp(EventLoop)
+   */
+  public Trigger dpadUp() {
+    return dpadUp(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad up button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the D-pad up button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger dpadUp(EventLoop loop) {
+    return button(Gamepad.Button.kDpadUp.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad down button's digital signal.
+   *
+   * @return a Trigger instance representing the D-pad down button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #dpadDown(EventLoop)
+   */
+  public Trigger dpadDown() {
+    return dpadDown(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad down button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the D-pad down button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger dpadDown(EventLoop loop) {
+    return button(Gamepad.Button.kDpadDown.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad left button's digital signal.
+   *
+   * @return a Trigger instance representing the D-pad left button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #dpadLeft(EventLoop)
+   */
+  public Trigger dpadLeft() {
+    return dpadLeft(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad left button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the D-pad left button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger dpadLeft(EventLoop loop) {
+    return button(Gamepad.Button.kDpadLeft.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad right button's digital signal.
+   *
+   * @return a Trigger instance representing the D-pad right button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #dpadRight(EventLoop)
+   */
+  public Trigger dpadRight() {
+    return dpadRight(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the D-pad right button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the D-pad right button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger dpadRight(EventLoop loop) {
+    return button(Gamepad.Button.kDpadRight.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 1 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 1 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc1(EventLoop)
+   */
+  public Trigger misc1() {
+    return misc1(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 1 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 1 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc1(EventLoop loop) {
+    return button(Gamepad.Button.kMisc1.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 1 button's digital signal.
+   *
+   * @return a Trigger instance representing the Right Paddle 1 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightPaddle1(EventLoop)
+   */
+  public Trigger rightPaddle1() {
+    return rightPaddle1(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 1 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Right Paddle 1 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger rightPaddle1(EventLoop loop) {
+    return button(Gamepad.Button.kRightPaddle1.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 1 button's digital signal.
+   *
+   * @return a Trigger instance representing the Left Paddle 1 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftPaddle1(EventLoop)
+   */
+  public Trigger leftPaddle1() {
+    return leftPaddle1(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 1 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Left Paddle 1 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger leftPaddle1(EventLoop loop) {
+    return button(Gamepad.Button.kLeftPaddle1.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 2 button's digital signal.
+   *
+   * @return a Trigger instance representing the Right Paddle 2 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #rightPaddle2(EventLoop)
+   */
+  public Trigger rightPaddle2() {
+    return rightPaddle2(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 2 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Right Paddle 2 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger rightPaddle2(EventLoop loop) {
+    return button(Gamepad.Button.kRightPaddle2.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 2 button's digital signal.
+   *
+   * @return a Trigger instance representing the Left Paddle 2 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #leftPaddle2(EventLoop)
+   */
+  public Trigger leftPaddle2() {
+    return leftPaddle2(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 2 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Left Paddle 2 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger leftPaddle2(EventLoop loop) {
+    return button(Gamepad.Button.kLeftPaddle2.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Touchpad button's digital signal.
+   *
+   * @return a Trigger instance representing the Touchpad button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #touchpad(EventLoop)
+   */
+  public Trigger touchpad() {
+    return touchpad(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Touchpad button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Touchpad button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger touchpad(EventLoop loop) {
+    return button(Gamepad.Button.kTouchpad.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 2 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 2 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc2(EventLoop)
+   */
+  public Trigger misc2() {
+    return misc2(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 2 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 2 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc2(EventLoop loop) {
+    return button(Gamepad.Button.kMisc2.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 3 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 3 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc3(EventLoop)
+   */
+  public Trigger misc3() {
+    return misc3(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 3 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 3 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc3(EventLoop loop) {
+    return button(Gamepad.Button.kMisc3.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 4 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 4 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc4(EventLoop)
+   */
+  public Trigger misc4() {
+    return misc4(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 4 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 4 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc4(EventLoop loop) {
+    return button(Gamepad.Button.kMisc4.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 5 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 5 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc5(EventLoop)
+   */
+  public Trigger misc5() {
+    return misc5(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 5 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 5 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc5(EventLoop loop) {
+    return button(Gamepad.Button.kMisc5.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 6 button's digital signal.
+   *
+   * @return a Trigger instance representing the Miscellaneous 6 button's digital signal attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #misc6(EventLoop)
+   */
+  public Trigger misc6() {
+    return misc6(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 6 button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return a Trigger instance representing the Miscellaneous 6 button's digital signal attached to
+   *     the given loop.
+   */
+  public Trigger misc6(EventLoop loop) {
+    return button(Gamepad.Button.kMisc6.value, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger
+   * will be true when the axis value is greater than {@code threshold}.
+   *
+   * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value
+   *     should be in the range [0, 1] where 0 is the unpressed state of the axis.
+   * @param loop the event loop instance to attach the Trigger to.
+   * @return a Trigger instance that is true when the left trigger's axis exceeds the provided
+   *     threshold, attached to the given event loop
+   */
+  public Trigger leftTrigger(double threshold, EventLoop loop) {
+    return axisGreaterThan(Gamepad.Axis.kLeftTrigger.value, threshold, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger
+   * will be true when the axis value is greater than {@code threshold}.
+   *
+   * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value
+   *     should be in the range [0, 1] where 0 is the unpressed state of the axis.
+   * @return a Trigger instance that is true when the left trigger's axis exceeds the provided
+   *     threshold, attached to the {@link CommandScheduler#getDefaultButtonLoop() default scheduler
+   *     button loop}.
+   */
+  public Trigger leftTrigger(double threshold) {
+    return leftTrigger(threshold, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger
+   * will be true when the axis value is greater than 0.5.
+   *
+   * @return a Trigger instance that is true when the left trigger's axis exceeds 0.5, attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   */
+  public Trigger leftTrigger() {
+    return leftTrigger(0.5);
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger
+   * will be true when the axis value is greater than {@code threshold}.
+   *
+   * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value
+   *     should be in the range [0, 1] where 0 is the unpressed state of the axis.
+   * @param loop the event loop instance to attach the Trigger to.
+   * @return a Trigger instance that is true when the right trigger's axis exceeds the provided
+   *     threshold, attached to the given event loop
+   */
+  public Trigger rightTrigger(double threshold, EventLoop loop) {
+    return axisGreaterThan(Gamepad.Axis.kRightTrigger.value, threshold, loop);
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger
+   * will be true when the axis value is greater than {@code threshold}.
+   *
+   * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value
+   *     should be in the range [0, 1] where 0 is the unpressed state of the axis.
+   * @return a Trigger instance that is true when the right trigger's axis exceeds the provided
+   *     threshold, attached to the {@link CommandScheduler#getDefaultButtonLoop() default scheduler
+   *     button loop}.
+   */
+  public Trigger rightTrigger(double threshold) {
+    return rightTrigger(threshold, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger
+   * will be true when the axis value is greater than 0.5.
+   *
+   * @return a Trigger instance that is true when the right trigger's axis exceeds 0.5, attached to
+   *     the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   */
+  public Trigger rightTrigger() {
+    return rightTrigger(0.5);
+  }
+
+  /**
+   * Get the X axis value of left side of the controller. Right is positive.
+   *
+   * @return The axis value.
+   */
+  public double getLeftX() {
+    return m_hid.getLeftX();
+  }
+
+  /**
+   * Get the Y axis value of left side of the controller. Back is positive.
+   *
+   * @return The axis value.
+   */
+  public double getLeftY() {
+    return m_hid.getLeftY();
+  }
+
+  /**
+   * Get the X axis value of right side of the controller. Right is positive.
+   *
+   * @return The axis value.
+   */
+  public double getRightX() {
+    return m_hid.getRightX();
+  }
+
+  /**
+   * Get the Y axis value of right side of the controller. Back is positive.
+   *
+   * @return The axis value.
+   */
+  public double getRightY() {
+    return m_hid.getRightY();
+  }
+
+  /**
+   * Get the left trigger axis value of the controller. Note that this axis is bound to the range of
+   * [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return The axis value.
+   */
+  public double getLeftTriggerAxis() {
+    return m_hid.getLeftTriggerAxis();
+  }
+
+  /**
+   * Get the right trigger axis value of the controller. Note that this axis is bound to the range
+   * of [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return The axis value.
+   */
+  public double getRightTriggerAxis() {
+    return m_hid.getRightTriggerAxis();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/CommandGenericHID.java

@@ -0,0 +1,320 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.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 edu.wpi.first.wpilibj2.command.CommandScheduler;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * A version of {@link GenericHID} with {@link Trigger} factories for command-based.
+ *
+ * @see GenericHID
+ */
+public class CommandGenericHID {
+  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 port The port index on the Driver Station that the device is plugged into.
+   */
+  public CommandGenericHID(int port) {
+    m_hid = new GenericHID(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
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #button(int, EventLoop)
+   */
+  public Trigger button(int button) {
+    return button(button, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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(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 CommandScheduler#getDefaultButtonLoop() 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, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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
+   *     CommandScheduler#getDefaultButtonLoop() 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(loop, () -> m_hid.getPOV(pov) == angle));
+  }
+
+  /**
+   * Constructs a Trigger instance based around the up direction of the default (index 0) POV on the
+   * HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler
+   * button loop}.
+   *
+   * @return a Trigger instance based around the up direction of a POV on the HID.
+   */
+  public Trigger povUp() {
+    return pov(POVDirection.Up);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the up right direction of the default (index 0) POV
+   * on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the up right direction of a POV on the HID.
+   */
+  public Trigger povUpRight() {
+    return pov(POVDirection.UpRight);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the right direction of the default (index 0) POV on
+   * the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the right direction of a POV on the HID.
+   */
+  public Trigger povRight() {
+    return pov(POVDirection.Right);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the down right direction of the default (index 0)
+   * POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the down right direction of a POV on the HID.
+   */
+  public Trigger povDownRight() {
+    return pov(POVDirection.DownRight);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the down direction of the default (index 0) POV on
+   * the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the down direction of a POV on the HID.
+   */
+  public Trigger povDown() {
+    return pov(POVDirection.Down);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the down left POV direction of the default (index 0)
+   * POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the down left POV direction of a POV on the HID.
+   */
+  public Trigger povDownLeft() {
+    return pov(POVDirection.DownLeft);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the left direction of the default (index 0) POV on
+   * the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the left direction of a POV on the HID.
+   */
+  public Trigger povLeft() {
+    return pov(POVDirection.Left);
+  }
+
+  /**
+   * Constructs a Trigger instance based around the up left direction of the default (index 0) POV
+   * on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   *
+   * @return a Trigger instance based around the up left direction 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 CommandScheduler#getDefaultButtonLoop() 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 CommandScheduler#getDefaultButtonLoop() 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, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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(loop, () -> getRawAxis(axis) < threshold));
+  }
+
+  /**
+   * Constructs a Trigger instance that is true when the axis value is less than {@code threshold},
+   * attached to {@link CommandScheduler#getDefaultButtonLoop() 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, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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(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(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 CommandScheduler#getDefaultButtonLoop() 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, CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/CommandJoystick.java

@@ -0,0 +1,263 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * 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);
+  }
+
+  /**
+   * 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 CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #trigger(EventLoop)
+   */
+  public Trigger trigger() {
+    return trigger(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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
+   *     CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * @see #top(EventLoop)
+   */
+  public Trigger top() {
+    return top(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * 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();
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/InternalButton.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/**
+ * 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.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+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());
+    this.m_pressed = state;
+    this.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);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/JoystickButton.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A {@link Trigger} that gets its state from a {@link GenericHID}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+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");
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/NetworkButton.java

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.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;
+
+/**
+ * A {@link Trigger} that uses a {@link NetworkTable} boolean field.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+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);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/POVButton.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.wpilibj.DriverStation.POVDirection;
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A {@link Trigger} that gets its state from a POV on a {@link GenericHID}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+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);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/RobotModeTriggers.java

@@ -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 edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * 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);
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java

@@ -0,0 +1,290 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import java.util.function.BooleanSupplier;
+
+/**
+ * This class provides an easy way to link commands to conditions.
+ *
+ * <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>This class is provided by the NewCommands VendorDep
+ */
+public class Trigger implements BooleanSupplier {
+  /** Functional interface for the body of a trigger binding. */
+  @FunctionalInterface
+  private interface BindingBody {
+    /**
+     * Executes the body of the binding.
+     *
+     * @param previous The previous state of the condition.
+     * @param current The current state of the condition.
+     */
+    void run(boolean previous, boolean current);
+  }
+
+  private final BooleanSupplier m_condition;
+  private final EventLoop m_loop;
+
+  /**
+   * Creates a new trigger based on the given condition.
+   *
+   * @param loop The loop instance that polls this trigger.
+   * @param condition the condition represented by this trigger
+   */
+  public Trigger(EventLoop loop, BooleanSupplier condition) {
+    m_loop = requireNonNullParam(loop, "loop", "Trigger");
+    m_condition = requireNonNullParam(condition, "condition", "Trigger");
+  }
+
+  /**
+   * Creates a new trigger based on the given condition.
+   *
+   * <p>Polled by the default scheduler button loop.
+   *
+   * @param condition the condition represented by this trigger
+   */
+  public Trigger(BooleanSupplier condition) {
+    this(CommandScheduler.getInstance().getDefaultButtonLoop(), condition);
+  }
+
+  /**
+   * Adds a binding to the EventLoop.
+   *
+   * @param body The body of the binding to add.
+   */
+  private void addBinding(BindingBody body) {
+    m_loop.bind(
+        new Runnable() {
+          private boolean m_previous = m_condition.getAsBoolean();
+
+          @Override
+          public void run() {
+            boolean current = m_condition.getAsBoolean();
+
+            body.run(m_previous, current);
+
+            m_previous = current;
+          }
+        });
+  }
+
+  /**
+   * Starts the command when the condition changes.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger onChange(Command command) {
+    requireNonNullParam(command, "command", "onChange");
+    addBinding(
+        (previous, current) -> {
+          if (previous != current) {
+            CommandScheduler.getInstance().schedule(command);
+          }
+        });
+    return this;
+  }
+
+  /**
+   * 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(
+        (previous, current) -> {
+          if (!previous && current) {
+            CommandScheduler.getInstance().schedule(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(
+        (previous, current) -> {
+          if (previous && !current) {
+            CommandScheduler.getInstance().schedule(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`. If the command
+   * should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileTrue(Command command) {
+    requireNonNullParam(command, "command", "whileTrue");
+    addBinding(
+        (previous, current) -> {
+          if (!previous && current) {
+            CommandScheduler.getInstance().schedule(command);
+          } else if (previous && !current) {
+            command.cancel();
+          }
+        });
+    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`. If the command
+   * should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileFalse(Command command) {
+    requireNonNullParam(command, "command", "whileFalse");
+    addBinding(
+        (previous, current) -> {
+          if (previous && !current) {
+            CommandScheduler.getInstance().schedule(command);
+          } else if (!previous && current) {
+            command.cancel();
+          }
+        });
+    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(
+        (previous, current) -> {
+          if (!previous && current) {
+            if (command.isScheduled()) {
+              command.cancel();
+            } else {
+              CommandScheduler.getInstance().schedule(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(
+        (previous, current) -> {
+          if (previous && !current) {
+            if (command.isScheduled()) {
+              command.cancel();
+            } else {
+              CommandScheduler.getInstance().schedule(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_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_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_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 seconds The debounce period.
+   * @return The debounced trigger (rising edges debounced only)
+   */
+  public Trigger debounce(double seconds) {
+    return debounce(seconds, 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 seconds The debounce period.
+   * @param type The debounce type.
+   * @return The debounced trigger.
+   */
+  public Trigger debounce(double seconds, Debouncer.DebounceType type) {
+    return new Trigger(
+        m_loop,
+        new BooleanSupplier() {
+          final Debouncer m_debouncer = new Debouncer(seconds, type);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_debouncer.calculate(m_condition.getAsBoolean());
+          }
+        });
+  }
+}

commandsv2/src/main/java/org/wpilib/command2/sysid/SysIdRoutine.java

@@ -0,0 +1,277 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+import static java.util.Map.entry;
+
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.units.measure.Velocity;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.Map;
+import java.util.function.Consumer;
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors.
+ *
+ * <p>A single subsystem may have multiple mechanisms, but mechanisms should not share test
+ * routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the
+ * log name of the recorded data is determined by the mechanism name.
+ *
+ * <p>The test state (e.g. "quasistatic-forward") is logged once per iteration during test
+ * execution, and once with state "none" when a test ends. Motor frames are logged every iteration
+ * during test execution.
+ *
+ * <p>Timestamps are not coordinated across data, so motor frames and test state tags may be
+ * recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log
+ * by using the test state flag to determine the timestamp range for each section of the test, and
+ * then extracts the motor frames within the valid timestamp ranges. If a given test was run
+ * multiple times in a single logfile, the user will need to select which of the tests to use for
+ * the fit in the analysis tool.
+ */
+public class SysIdRoutine extends SysIdRoutineLog {
+  private final Config m_config;
+  private final Mechanism m_mechanism;
+  private final Consumer<State> m_recordState;
+
+  /**
+   * Create a new SysId characterization routine.
+   *
+   * @param config Hardware-independent parameters for the SysId routine.
+   * @param mechanism Hardware interface for the SysId routine.
+   */
+  public SysIdRoutine(Config config, Mechanism mechanism) {
+    super(mechanism.m_name);
+    m_config = config;
+    m_mechanism = mechanism;
+    m_recordState = config.m_recordState != null ? config.m_recordState : this::recordState;
+  }
+
+  /** Hardware-independent configuration for a SysId test routine. */
+  public static class Config {
+    /** The voltage ramp rate used for quasistatic test routines. */
+    public final Velocity<VoltageUnit> m_rampRate;
+
+    /** The step voltage output used for dynamic test routines. */
+    public final Voltage m_stepVoltage;
+
+    /** Safety timeout for the test routine commands. */
+    public final Time m_timeout;
+
+    /** Optional handle for recording test state in a third-party logging solution. */
+    public final Consumer<State> m_recordState;
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     * @param recordState Optional handle for recording test state in a third-party logging
+     *     solution. If provided, the test routine state will be passed to this callback instead of
+     *     logged in WPILog.
+     */
+    public Config(
+        Velocity<VoltageUnit> rampRate,
+        Voltage stepVoltage,
+        Time timeout,
+        Consumer<State> recordState) {
+      m_rampRate = rampRate != null ? rampRate : Volts.of(1).per(Second);
+      m_stepVoltage = stepVoltage != null ? stepVoltage : Volts.of(7);
+      m_timeout = timeout != null ? timeout : Seconds.of(10);
+      m_recordState = recordState;
+    }
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     */
+    public Config(Velocity<VoltageUnit> rampRate, Voltage stepVoltage, Time timeout) {
+      this(rampRate, stepVoltage, timeout, null);
+    }
+
+    /**
+     * Create a default configuration for a SysId test routine with all default settings.
+     *
+     * <p>rampRate: 1 volt/sec
+     *
+     * <p>stepVoltage: 7 volts
+     *
+     * <p>timeout: 10 seconds
+     */
+    public Config() {
+      this(null, null, null, null);
+    }
+  }
+
+  /**
+   * A mechanism to be characterized by a SysId routine. Defines callbacks needed for the SysId test
+   * routine to control and record data from the mechanism.
+   */
+  public static class Mechanism {
+    /** Sends the SysId-specified drive signal to the mechanism motors during test routines. */
+    public final Consumer<? super Voltage> m_drive;
+
+    /**
+     * Returns measured data (voltages, positions, velocities) of the mechanism motors during test
+     * routines.
+     */
+    public final Consumer<SysIdRoutineLog> m_log;
+
+    /** The subsystem containing the motor(s) that is (or are) being characterized. */
+    public final Subsystem m_subsystem;
+
+    /** The name of the mechanism being tested. */
+    public final String m_name;
+
+    /**
+     * Create a new mechanism specification for a SysId routine.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data of the mechanism motors during test routines. To return
+     *     data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+     *     call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+     *     `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+     *     multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands.
+     * @param name The name of the mechanism being tested. Will be appended to the log entry title
+     *     for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of
+     *     the subsystem if left null.
+     */
+    public Mechanism(
+        Consumer<Voltage> drive, Consumer<SysIdRoutineLog> log, Subsystem subsystem, String name) {
+      m_drive = drive;
+      m_log = log != null ? log : l -> {};
+      m_subsystem = subsystem;
+      m_name = name != null ? name : subsystem.getName();
+    }
+
+    /**
+     * Create a new mechanism specification for a SysId routine. Defaults the mechanism name to the
+     * subsystem name.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data of the mechanism motors during test routines. To return
+     *     data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+     *     call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+     *     `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+     *     multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands. The subsystem's `name`
+     *     will be appended to the log entry title for the routine's test state, e.g.
+     *     "sysid-test-state-subsystem".
+     */
+    public Mechanism(Consumer<Voltage> drive, Consumer<SysIdRoutineLog> log, Subsystem subsystem) {
+      this(drive, log, subsystem, null);
+    }
+  }
+
+  /** Motor direction for a SysId test. */
+  public enum Direction {
+    /** Forward. */
+    kForward,
+    /** Reverse. */
+    kReverse
+  }
+
+  /**
+   * Returns a command to run a quasistatic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command quasistatic(Direction direction) {
+    State state;
+    if (direction == Direction.kForward) {
+      state = State.kQuasistaticForward;
+    } else { // if (direction == Direction.kReverse) {
+      state = State.kQuasistaticReverse;
+    }
+
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+
+    Timer timer = new Timer();
+    return m_mechanism
+        .m_subsystem
+        .runOnce(timer::restart)
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(
+                      (Voltage) m_config.m_rampRate.times(Seconds.of(timer.get() * outputSign)));
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+              timer.stop();
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+
+  /**
+   * Returns a command to run a dynamic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command dynamic(Direction direction) {
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+    State state =
+        Map.ofEntries(
+                entry(Direction.kForward, State.kDynamicForward),
+                entry(Direction.kReverse, State.kDynamicReverse))
+            .get(direction);
+    Voltage[] output = {Volts.zero()};
+
+    return m_mechanism
+        .m_subsystem
+        .runOnce(() -> output[0] = m_config.m_stepVoltage.times(outputSign))
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(output[0]);
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+}