SCRIPT Move subprojects

commandsv2/src/main/native/cpp/frc2/command/Command.cpp

@@ -0,0 +1,233 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/Command.h"
+
+#include <string>
+#include <utility>
+
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/CommandScheduler.h"
+
+using namespace frc2;
+
+Command::Command() {
+  wpi::SendableRegistry::Add(this, GetTypeName(*this));
+}
+
+Command::~Command() {
+  CommandScheduler::GetInstance().Cancel(this);
+}
+
+Command& Command::operator=(const Command& rhs) {
+  SetComposed(false);
+  return *this;
+}
+
+void Command::Initialize() {}
+void Command::Execute() {}
+void Command::End(bool interrupted) {}
+
+wpi::SmallSet<Subsystem*, 4> Command::GetRequirements() const {
+  return m_requirements;
+}
+
+void Command::AddRequirements(Requirements requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(Subsystem* requirement) {
+  m_requirements.insert(requirement);
+}
+
+void Command::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
+}
+
+std::string Command::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+std::string Command::GetSubsystem() const {
+  return wpi::SendableRegistry::GetSubsystem(this);
+}
+
+void Command::SetSubsystem(std::string_view subsystem) {
+  wpi::SendableRegistry::SetSubsystem(this, subsystem);
+}
+
+CommandPtr Command::WithTimeout(units::second_t duration) && {
+  return std::move(*this).ToPtr().WithTimeout(duration);
+}
+
+CommandPtr Command::Until(std::function<bool()> condition) && {
+  return std::move(*this).ToPtr().Until(std::move(condition));
+}
+
+CommandPtr Command::OnlyWhile(std::function<bool()> condition) && {
+  return std::move(*this).ToPtr().OnlyWhile(std::move(condition));
+}
+
+CommandPtr Command::IgnoringDisable(bool doesRunWhenDisabled) && {
+  return std::move(*this).ToPtr().IgnoringDisable(doesRunWhenDisabled);
+}
+
+CommandPtr Command::WithInterruptBehavior(
+    InterruptionBehavior interruptBehavior) && {
+  return std::move(*this).ToPtr().WithInterruptBehavior(interruptBehavior);
+}
+
+CommandPtr Command::BeforeStarting(std::function<void()> toRun,
+                                   Requirements requirements) && {
+  return std::move(*this).ToPtr().BeforeStarting(std::move(toRun),
+                                                 requirements);
+}
+
+CommandPtr Command::BeforeStarting(CommandPtr&& before) && {
+  return std::move(*this).ToPtr().BeforeStarting(std::move(before));
+}
+
+CommandPtr Command::AndThen(std::function<void()> toRun,
+                            Requirements requirements) && {
+  return std::move(*this).ToPtr().AndThen(std::move(toRun), requirements);
+}
+
+CommandPtr Command::AndThen(CommandPtr&& next) && {
+  return std::move(*this).ToPtr().AndThen(std::move(next));
+}
+
+CommandPtr Command::Repeatedly() && {
+  return std::move(*this).ToPtr().Repeatedly();
+}
+
+CommandPtr Command::AsProxy() && {
+  return std::move(*this).ToPtr().AsProxy();
+}
+
+CommandPtr Command::Unless(std::function<bool()> condition) && {
+  return std::move(*this).ToPtr().Unless(std::move(condition));
+}
+
+CommandPtr Command::OnlyIf(std::function<bool()> condition) && {
+  return std::move(*this).ToPtr().OnlyIf(std::move(condition));
+}
+
+CommandPtr Command::WithDeadline(CommandPtr&& deadline) && {
+  return std::move(*this).ToPtr().WithDeadline(std::move(deadline));
+}
+
+CommandPtr Command::DeadlineFor(CommandPtr&& parallel) && {
+  return std::move(*this).ToPtr().DeadlineFor(std::move(parallel));
+}
+
+CommandPtr Command::AlongWith(CommandPtr&& parallel) && {
+  return std::move(*this).ToPtr().AlongWith(std::move(parallel));
+}
+
+CommandPtr Command::RaceWith(CommandPtr&& parallel) && {
+  return std::move(*this).ToPtr().RaceWith(std::move(parallel));
+}
+
+CommandPtr Command::FinallyDo(std::function<void(bool)> end) && {
+  return std::move(*this).ToPtr().FinallyDo(std::move(end));
+}
+
+CommandPtr Command::FinallyDo(std::function<void()> end) && {
+  return std::move(*this).ToPtr().FinallyDo(std::move(end));
+}
+
+CommandPtr Command::HandleInterrupt(std::function<void()> handler) && {
+  return std::move(*this).ToPtr().HandleInterrupt(std::move(handler));
+}
+
+CommandPtr Command::WithName(std::string_view name) && {
+  return std::move(*this).ToPtr().WithName(name);
+}
+
+void Command::Schedule() {
+  CommandScheduler::GetInstance().Schedule(this);
+}
+
+void Command::Cancel() {
+  CommandScheduler::GetInstance().Cancel(this);
+}
+
+bool Command::IsScheduled() const {
+  return CommandScheduler::GetInstance().IsScheduled(this);
+}
+
+bool Command::HasRequirement(Subsystem* requirement) const {
+  bool hasRequirement = false;
+  for (auto&& subsystem : GetRequirements()) {
+    hasRequirement |= requirement == subsystem;
+  }
+  return hasRequirement;
+}
+
+bool Command::IsComposed() const {
+  return GetPreviousCompositionSite().has_value();
+}
+
+void Command::SetComposed(bool isComposed) {
+  if (isComposed) {
+    m_previousComposition = wpi::GetStackTrace(1);
+  } else {
+    m_previousComposition.reset();
+  }
+}
+
+std::optional<std::string> Command::GetPreviousCompositionSite() const {
+  return m_previousComposition;
+}
+
+void Command::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
+  builder.AddBooleanProperty(
+      "running", [this] { return IsScheduled(); },
+      [this](bool value) {
+        bool isScheduled = IsScheduled();
+        if (value && !isScheduled) {
+          CommandScheduler::GetInstance().Schedule(this);
+        } else if (!value && isScheduled) {
+          Cancel();
+        }
+      });
+  builder.AddBooleanProperty(
+      ".isParented", [this] { return IsComposed(); }, nullptr);
+  builder.AddStringProperty(
+      "interruptBehavior",
+      [this] {
+        switch (GetInterruptionBehavior()) {
+          case Command::InterruptionBehavior::kCancelIncoming:
+            return "kCancelIncoming";
+          case Command::InterruptionBehavior::kCancelSelf:
+            return "kCancelSelf";
+          default:
+            return "Invalid";
+        }
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr);
+}
+
+namespace frc2 {
+bool RequirementsDisjoint(Command* first, Command* second) {
+  bool disjoint = true;
+  auto&& requirements = second->GetRequirements();
+  for (auto&& requirement : first->GetRequirements()) {
+    disjoint &= requirements.find(requirement) == requirements.end();
+  }
+  return disjoint;
+}
+}  // namespace frc2

commandsv2/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -0,0 +1,303 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/CommandPtr.h"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <frc/Errors.h>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/ProxyCommand.h"
+#include "frc2/command/RepeatCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/WrapperCommand.h"
+
+using namespace frc2;
+
+CommandPtr::CommandPtr(std::unique_ptr<Command>&& command)
+    : m_ptr(std::move(command)) {
+  AssertValid();
+}
+
+CommandPtr::CommandPtr(CommandPtr&& rhs) {
+  m_ptr = std::move(rhs.m_ptr);
+  AssertValid();
+  rhs.m_moveOutSite = wpi::GetStackTrace(1);
+}
+
+void CommandPtr::AssertValid() const {
+  if (!m_ptr) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Moved-from CommandPtr object used!\nMoved out at:\n{}",
+                        m_moveOutSite);
+  }
+}
+
+CommandPtr CommandPtr::Repeatedly() && {
+  AssertValid();
+  m_ptr = std::make_unique<RepeatCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AsProxy() && {
+  AssertValid();
+  m_ptr = std::make_unique<ProxyCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+class RunsWhenDisabledCommand : public WrapperCommand {
+ public:
+  RunsWhenDisabledCommand(std::unique_ptr<Command>&& command,
+                          bool doesRunWhenDisabled)
+      : WrapperCommand(std::move(command)),
+        m_runsWhenDisabled(doesRunWhenDisabled) {}
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ private:
+  bool m_runsWhenDisabled;
+};
+
+CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && {
+  AssertValid();
+  m_ptr = std::make_unique<RunsWhenDisabledCommand>(std::move(m_ptr),
+                                                    doesRunWhenDisabled);
+  return std::move(*this);
+}
+
+using InterruptionBehavior = Command::InterruptionBehavior;
+class InterruptBehaviorCommand : public WrapperCommand {
+ public:
+  InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
+                           InterruptionBehavior interruptBehavior)
+      : WrapperCommand(std::move(command)),
+        m_interruptBehavior(interruptBehavior) {}
+
+  InterruptionBehavior GetInterruptionBehavior() const override {
+    return m_interruptBehavior;
+  }
+
+ private:
+  InterruptionBehavior m_interruptBehavior;
+};
+
+CommandPtr CommandPtr::WithInterruptBehavior(
+    InterruptionBehavior interruptBehavior) && {
+  AssertValid();
+  m_ptr = std::make_unique<InterruptBehaviorCommand>(std::move(m_ptr),
+                                                     interruptBehavior);
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AndThen(std::function<void()> toRun,
+                               Requirements requirements) && {
+  AssertValid();
+  return std::move(*this).AndThen(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
+CommandPtr CommandPtr::AndThen(CommandPtr&& next) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::move(next).Unwrap());
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::BeforeStarting(std::function<void()> toRun,
+                                      Requirements requirements) && {
+  AssertValid();
+  return std::move(*this).BeforeStarting(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
+CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(before).Unwrap());
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitCommand>(duration));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::OnlyWhile(std::function<bool()> condition) && {
+  AssertValid();
+  return std::move(*this).Until(std::not_fn(std::move(condition)));
+}
+
+CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
+  AssertValid();
+  m_ptr = std::make_unique<ConditionalCommand>(
+      std::make_unique<InstantCommand>(), std::move(m_ptr),
+      std::move(condition));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::OnlyIf(std::function<bool()> condition) && {
+  AssertValid();
+  return std::move(*this).Unless(std::not_fn(std::move(condition)));
+}
+
+CommandPtr CommandPtr::WithDeadline(CommandPtr&& deadline) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<ParallelDeadlineGroup>(std::move(deadline).Unwrap(),
+                                                  std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr =
+      std::make_unique<ParallelDeadlineGroup>(std::move(m_ptr), std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::DeadlineFor(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr =
+      std::make_unique<ParallelDeadlineGroup>(std::move(m_ptr), std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelCommandGroup>(std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::RaceWith(CommandPtr&& parallel) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(vec));
+  return std::move(*this);
+}
+
+namespace {
+class FinallyCommand : public WrapperCommand {
+ public:
+  FinallyCommand(std::unique_ptr<Command>&& command,
+                 std::function<void(bool)> end)
+      : WrapperCommand(std::move(command)), m_end(std::move(end)) {}
+
+  void End(bool interrupted) override {
+    WrapperCommand::End(interrupted);
+    m_end(interrupted);
+  }
+
+ private:
+  std::function<void(bool)> m_end;
+};
+}  // namespace
+
+CommandPtr CommandPtr::FinallyDo(std::function<void(bool)> end) && {
+  AssertValid();
+  m_ptr = std::make_unique<FinallyCommand>(std::move(m_ptr), std::move(end));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::FinallyDo(std::function<void()> end) && {
+  AssertValid();
+  return std::move(*this).FinallyDo(
+      [endHandler = std::move(end)](bool interrupted) { endHandler(); });
+}
+
+CommandPtr CommandPtr::HandleInterrupt(std::function<void()> handler) && {
+  AssertValid();
+  return std::move(*this).FinallyDo(
+      [handler = std::move(handler)](bool interrupted) {
+        if (interrupted) {
+          handler();
+        }
+      });
+}
+
+CommandPtr CommandPtr::WithName(std::string_view name) && {
+  AssertValid();
+  WrapperCommand wrapper{std::move(m_ptr)};
+  wrapper.SetName(name);
+  return std::move(wrapper).ToPtr();
+}
+
+Command* CommandPtr::get() const& {
+  AssertValid();
+  return m_ptr.get();
+}
+
+std::unique_ptr<Command> CommandPtr::Unwrap() && {
+  AssertValid();
+  return std::move(m_ptr);
+}
+
+void CommandPtr::Schedule() const& {
+  AssertValid();
+  CommandScheduler::GetInstance().Schedule(*this);
+}
+
+void CommandPtr::Cancel() const& {
+  AssertValid();
+  CommandScheduler::GetInstance().Cancel(*this);
+}
+
+bool CommandPtr::IsScheduled() const& {
+  AssertValid();
+  return CommandScheduler::GetInstance().IsScheduled(*this);
+}
+
+bool CommandPtr::HasRequirement(Subsystem* requirement) const& {
+  AssertValid();
+  return m_ptr->HasRequirement(requirement);
+}
+
+CommandPtr::operator bool() const& {
+  return m_ptr.operator bool();
+}
+
+std::vector<std::unique_ptr<Command>> CommandPtr::UnwrapVector(
+    std::vector<CommandPtr>&& vec) {
+  std::vector<std::unique_ptr<Command>> ptrs;
+  for (auto&& ptr : vec) {
+    ptrs.emplace_back(std::move(ptr).Unwrap());
+  }
+  return ptrs;
+}

commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -0,0 +1,527 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <cstdio>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <frc/RobotBase.h>
+#include <frc/RobotState.h>
+#include <frc/TimedRobot.h>
+#include <hal/HALBase.h>
+#include <hal/UsageReporting.h>
+#include <networktables/IntegerArrayTopic.h>
+#include <networktables/StringArrayTopic.h>
+#include <wpi/DenseMap.h>
+#include <wpi/SmallVector.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+  // A set of the currently-running commands.
+  wpi::SmallSet<Command*, 12> scheduledCommands;
+
+  // A map from required subsystems to their requiring commands.  Also used as a
+  // set of the currently-required subsystems.
+  wpi::DenseMap<Subsystem*, Command*> requirements;
+
+  // A map from subsystems registered with the scheduler to their default
+  // commands.  Also used as a list of currently-registered subsystems.
+  wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> subsystems;
+
+  frc::EventLoop defaultButtonLoop;
+  // The set of currently-registered buttons that will be polled every
+  // iteration.
+  frc::EventLoop* activeButtonLoop{&defaultButtonLoop};
+
+  bool disabled{false};
+
+  // Lists of user-supplied actions to be executed on scheduling events for
+  // every command.
+  wpi::SmallVector<Action, 4> initActions;
+  wpi::SmallVector<Action, 4> executeActions;
+  wpi::SmallVector<InterruptAction, 4> interruptActions;
+  wpi::SmallVector<Action, 4> finishActions;
+
+  // Map of Command* -> CommandPtr for CommandPtrs transferred to the scheduler
+  // via Schedule(CommandPtr&&). These are erased (destroyed) at the very end of
+  // the loop cycle when the command lifecycle is complete.
+  wpi::DenseMap<Command*, CommandPtr> ownedCommands;
+};
+
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+  return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler()
+    : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] {
+        std::puts("CommandScheduler loop time overrun.");
+      }) {
+  HAL_ReportUsage("CommandScheduler", "");
+  wpi::SendableRegistry::Add(this, "Scheduler");
+}
+
+CommandScheduler::~CommandScheduler() {
+  wpi::SendableRegistry::Remove(this);
+  std::unique_ptr<Impl>().swap(m_impl);
+}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+  static CommandScheduler scheduler;
+  return scheduler;
+}
+
+void CommandScheduler::SetPeriod(units::second_t period) {
+  m_watchdog.SetTimeout(period);
+}
+
+frc::EventLoop* CommandScheduler::GetActiveButtonLoop() const {
+  return m_impl->activeButtonLoop;
+}
+
+void CommandScheduler::SetActiveButtonLoop(frc::EventLoop* loop) {
+  m_impl->activeButtonLoop = loop;
+}
+
+frc::EventLoop* CommandScheduler::GetDefaultButtonLoop() const {
+  return &(m_impl->defaultButtonLoop);
+}
+
+void CommandScheduler::Schedule(Command* command) {
+  RequireUngrouped(command);
+
+  if (m_impl->disabled || m_impl->scheduledCommands.contains(command) ||
+      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled())) {
+    return;
+  }
+
+  const auto& requirements = command->GetRequirements();
+
+  wpi::SmallVector<Command*, 8> intersection;
+
+  bool isDisjoint = true;
+  bool allInterruptible = true;
+  for (auto&& i1 : m_impl->requirements) {
+    if (requirements.find(i1.first) != requirements.end()) {
+      isDisjoint = false;
+      allInterruptible &= (i1.second->GetInterruptionBehavior() ==
+                           Command::InterruptionBehavior::kCancelSelf);
+      intersection.emplace_back(i1.second);
+    }
+  }
+
+  if (isDisjoint || allInterruptible) {
+    if (allInterruptible) {
+      for (auto&& cmdToCancel : intersection) {
+        Cancel(cmdToCancel, std::make_optional(command));
+      }
+    }
+    m_impl->scheduledCommands.insert(command);
+    for (auto&& requirement : requirements) {
+      m_impl->requirements[requirement] = command;
+    }
+    command->Initialize();
+    for (auto&& action : m_impl->initActions) {
+      action(*command);
+    }
+    m_watchdog.AddEpoch(command->GetName() + ".Initialize()");
+  }
+}
+
+void CommandScheduler::Schedule(std::span<Command* const> commands) {
+  for (auto command : commands) {
+    Schedule(command);
+  }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(command);
+  }
+}
+
+void CommandScheduler::Schedule(const CommandPtr& command) {
+  Schedule(command.get());
+}
+
+void CommandScheduler::Schedule(CommandPtr&& command) {
+  auto ptr = command.get();
+  m_impl->ownedCommands.try_emplace(ptr, std::move(command));
+  Schedule(ptr);
+}
+
+void CommandScheduler::Run() {
+  if (m_impl->disabled) {
+    return;
+  }
+
+  m_watchdog.Reset();
+
+  // Run the periodic method of all registered subsystems.
+  for (auto&& subsystem : m_impl->subsystems) {
+    subsystem.getFirst()->Periodic();
+    if constexpr (frc::RobotBase::IsSimulation()) {
+      subsystem.getFirst()->SimulationPeriodic();
+    }
+    m_watchdog.AddEpoch(subsystem.getFirst()->GetName() + ".Periodic()");
+  }
+
+  // Cache the active instance to avoid concurrency problems if SetActiveLoop()
+  // is called from inside the button bindings.
+  frc::EventLoop* loopCache = m_impl->activeButtonLoop;
+  // Poll buttons for new commands to add.
+  loopCache->Poll();
+  m_watchdog.AddEpoch("buttons.Run()");
+
+  bool isDisabled = frc::RobotState::IsDisabled();
+  // create a new set to avoid iterator invalidation.
+  for (Command* command : wpi::SmallSet(m_impl->scheduledCommands)) {
+    if (!IsScheduled(command)) {
+      continue;  // skip as the normal scheduledCommands was modified
+    }
+
+    if (isDisabled && !command->RunsWhenDisabled()) {
+      Cancel(command, std::nullopt);
+      continue;
+    }
+
+    command->Execute();
+    for (auto&& action : m_impl->executeActions) {
+      action(*command);
+    }
+    m_watchdog.AddEpoch(command->GetName() + ".Execute()");
+
+    if (command->IsFinished()) {
+      m_impl->scheduledCommands.erase(command);
+      command->End(false);
+      for (auto&& action : m_impl->finishActions) {
+        action(*command);
+      }
+
+      for (auto&& requirement : command->GetRequirements()) {
+        m_impl->requirements.erase(requirement);
+      }
+
+      m_watchdog.AddEpoch(command->GetName() + ".End(false)");
+      // remove owned commands after everything else is done
+      m_impl->ownedCommands.erase(command);
+    }
+  }
+
+  // Add default commands for un-required registered subsystems.
+  for (auto&& subsystem : m_impl->subsystems) {
+    auto s = m_impl->requirements.find(subsystem.getFirst());
+    if (s == m_impl->requirements.end() && subsystem.getSecond()) {
+      Schedule({subsystem.getSecond().get()});
+    }
+  }
+
+  m_watchdog.Disable();
+  if (m_watchdog.IsExpired()) {
+    m_watchdog.PrintEpochs();
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+  if (m_impl->subsystems.find(subsystem) != m_impl->subsystems.end()) {
+    std::puts("Tried to register an already-registered subsystem");
+    return;
+  }
+
+  m_impl->subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+  auto s = m_impl->subsystems.find(subsystem);
+  if (s != m_impl->subsystems.end()) {
+    m_impl->subsystems.erase(s);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(
+    std::span<Subsystem* const> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    std::span<Subsystem* const> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterAllSubsystems() {
+  m_impl->subsystems.clear();
+}
+
+void CommandScheduler::SetDefaultCommand(Subsystem* subsystem,
+                                         CommandPtr&& defaultCommand) {
+  if (!defaultCommand.get()->HasRequirement(subsystem)) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Default commands must require their subsystem!");
+  }
+  RequireUngrouped(defaultCommand.get());
+
+  SetDefaultCommandImpl(subsystem, std::move(defaultCommand).Unwrap());
+}
+
+void CommandScheduler::RemoveDefaultCommand(Subsystem* subsystem) {
+  m_impl->subsystems[subsystem] = nullptr;
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+  auto&& find = m_impl->subsystems.find(subsystem);
+  if (find != m_impl->subsystems.end()) {
+    return find->second.get();
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Cancel(Command* command,
+                              std::optional<Command*> interruptor) {
+  if (!m_impl) {
+    return;
+  }
+  if (!IsScheduled(command)) {
+    return;
+  }
+  m_impl->scheduledCommands.erase(command);
+  command->End(true);
+  for (auto&& action : m_impl->interruptActions) {
+    action(*command, interruptor);
+  }
+  for (auto&& requirement : m_impl->requirements) {
+    if (requirement.second == command) {
+      m_impl->requirements.erase(requirement.first);
+    }
+  }
+  m_watchdog.AddEpoch(command->GetName() + ".End(true)");
+}
+
+void CommandScheduler::Cancel(Command* command) {
+  Cancel(command, std::nullopt);
+}
+
+void CommandScheduler::Cancel(const CommandPtr& command) {
+  Cancel(command.get());
+}
+
+void CommandScheduler::Cancel(std::span<Command* const> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::CancelAll() {
+  wpi::SmallVector<Command*, 16> commands;
+  for (auto&& command : m_impl->scheduledCommands) {
+    commands.emplace_back(command);
+  }
+  Cancel(commands);
+}
+
+bool CommandScheduler::IsScheduled(
+    std::span<const Command* const> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(
+    std::initializer_list<const Command*> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+  return m_impl->scheduledCommands.contains(command);
+}
+
+bool CommandScheduler::IsScheduled(const CommandPtr& command) const {
+  return m_impl->scheduledCommands.contains(command.get());
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+  auto find = m_impl->requirements.find(subsystem);
+  if (find != m_impl->requirements.end()) {
+    return find->second;
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Disable() {
+  m_impl->disabled = true;
+}
+
+void CommandScheduler::Enable() {
+  m_impl->disabled = false;
+}
+
+void CommandScheduler::PrintWatchdogEpochs() {
+  m_watchdog.PrintEpochs();
+}
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+  m_impl->initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+  m_impl->executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+  m_impl->interruptActions.emplace_back(
+      [action = std::move(action)](const Command& command,
+                                   const std::optional<Command*>& interruptor) {
+        action(command);
+      });
+}
+
+void CommandScheduler::OnCommandInterrupt(InterruptAction action) {
+  m_impl->interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+  m_impl->finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::RequireUngrouped(const Command* command) {
+  auto stacktrace = command->GetPreviousCompositionSite();
+  if (stacktrace.has_value()) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands that have been composed may not be added to "
+                        "another composition or scheduled individually!"
+                        "\nOriginally composed at:\n{}",
+                        stacktrace.value());
+  }
+}
+
+void CommandScheduler::RequireUngrouped(
+    std::span<const std::unique_ptr<Command>> commands) {
+  for (auto&& command : commands) {
+    RequireUngrouped(command.get());
+  }
+}
+
+void CommandScheduler::RequireUngrouped(
+    std::initializer_list<const Command*> commands) {
+  for (auto&& command : commands) {
+    RequireUngrouped(command);
+  }
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(const Command* command) {
+  if (IsScheduled(command)) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands that have been scheduled individually may "
+                        "not be added to another composition!");
+  }
+  RequireUngrouped(command);
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+    std::span<const std::unique_ptr<Command>> commands) {
+  for (auto&& command : commands) {
+    RequireUngroupedAndUnscheduled(command.get());
+  }
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+    std::initializer_list<const Command*> commands) {
+  for (auto&& command : commands) {
+    RequireUngroupedAndUnscheduled(command);
+  }
+}
+
+void CommandScheduler::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Scheduler");
+  builder.AddStringArrayProperty(
+      "Names",
+      [this]() mutable {
+        std::vector<std::string> names;
+        for (Command* command : m_impl->scheduledCommands) {
+          names.emplace_back(command->GetName());
+        }
+        return names;
+      },
+      nullptr);
+  builder.AddIntegerArrayProperty(
+      "Ids",
+      [this]() mutable {
+        std::vector<int64_t> ids;
+        for (Command* command : m_impl->scheduledCommands) {
+          uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command);
+          ids.emplace_back(static_cast<int64_t>(ptrTmp));
+        }
+        return ids;
+      },
+      nullptr);
+  builder.AddIntegerArrayProperty(
+      "Cancel", []() { return std::vector<int64_t>{}; },
+      [this](std::span<const int64_t> toCancel) mutable {
+        for (auto cancel : toCancel) {
+          uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+          Command* command = reinterpret_cast<Command*>(ptrTmp);
+          if (m_impl->scheduledCommands.find(command) !=
+              m_impl->scheduledCommands.end()) {
+            Cancel(command);
+          }
+        }
+      });
+}
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+                                             std::unique_ptr<Command> command) {
+  if (command->GetInterruptionBehavior() ==
+      Command::InterruptionBehavior::kCancelIncoming) {
+    std::puts(
+        "Registering a non-interruptible default command!\n"
+        "This will likely prevent any other commands from "
+        "requiring this subsystem.");
+    // Warn, but allow -- there might be a use case for this.
+  }
+  m_impl->subsystems[subsystem] = std::move(command);
+}

commandsv2/src/main/native/cpp/frc2/command/Commands.cpp

@@ -0,0 +1,137 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/Commands.h"
+
+#include <utility>
+#include <vector>
+
+#include <wpi/FunctionExtras.h>
+#include <wpi/deprecated.h>
+
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/DeferredCommand.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PrintCommand.h"
+#include "frc2/command/ProxyCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+// Factories
+
+CommandPtr cmd::None() {
+  return InstantCommand().ToPtr();
+}
+
+CommandPtr cmd::Idle(Requirements requirements) {
+  return Run([] {}, requirements);
+}
+
+CommandPtr cmd::RunOnce(std::function<void()> action,
+                        Requirements requirements) {
+  return InstantCommand(std::move(action), requirements).ToPtr();
+}
+
+CommandPtr cmd::Run(std::function<void()> action, Requirements requirements) {
+  return RunCommand(std::move(action), requirements).ToPtr();
+}
+
+CommandPtr cmd::StartEnd(std::function<void()> start, std::function<void()> end,
+                         Requirements requirements) {
+  return FunctionalCommand(
+             std::move(start), [] {},
+             [end = std::move(end)](bool interrupted) { end(); },
+             [] { return false; }, requirements)
+      .ToPtr();
+}
+
+CommandPtr cmd::RunEnd(std::function<void()> run, std::function<void()> end,
+                       Requirements requirements) {
+  return FunctionalCommand([] {}, std::move(run),
+                           [end = std::move(end)](bool interrupted) { end(); },
+                           [] { return false; }, requirements)
+      .ToPtr();
+}
+
+CommandPtr cmd::StartRun(std::function<void()> start, std::function<void()> run,
+                         Requirements requirements) {
+  return FunctionalCommand(
+             std::move(start), std::move(run), [](bool interrupted) {},
+             [] { return false; }, requirements)
+      .ToPtr();
+}
+
+CommandPtr cmd::Print(std::string_view msg) {
+  return PrintCommand(msg).ToPtr();
+}
+
+CommandPtr cmd::DeferredProxy(wpi::unique_function<Command*()> supplier) {
+  return Defer(
+      [supplier = std::move(supplier)]() mutable {
+        // There is no non-owning version of AsProxy(), so use the non-owning
+        // ProxyCommand constructor instead.
+        return ProxyCommand{supplier()}.ToPtr();
+      },
+      {});
+}
+
+CommandPtr cmd::DeferredProxy(wpi::unique_function<CommandPtr()> supplier) {
+  return Defer([supplier = std::move(
+                    supplier)]() mutable { return supplier().AsProxy(); },
+               {});
+}
+
+CommandPtr cmd::Wait(units::second_t duration) {
+  return WaitCommand(duration).ToPtr();
+}
+
+CommandPtr cmd::WaitUntil(std::function<bool()> condition) {
+  return WaitUntilCommand(condition).ToPtr();
+}
+
+CommandPtr cmd::Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
+                       std::function<bool()> selector) {
+  return ConditionalCommand(std::move(onTrue).Unwrap(),
+                            std::move(onFalse).Unwrap(), std::move(selector))
+      .ToPtr();
+}
+
+CommandPtr cmd::Defer(wpi::unique_function<CommandPtr()> supplier,
+                      Requirements requirements) {
+  return DeferredCommand(std::move(supplier), requirements).ToPtr();
+}
+
+CommandPtr cmd::Sequence(std::vector<CommandPtr>&& commands) {
+  return SequentialCommandGroup(CommandPtr::UnwrapVector(std::move(commands)))
+      .ToPtr();
+}
+
+CommandPtr cmd::RepeatingSequence(std::vector<CommandPtr>&& commands) {
+  return Sequence(std::move(commands)).Repeatedly();
+}
+
+CommandPtr cmd::Parallel(std::vector<CommandPtr>&& commands) {
+  return ParallelCommandGroup(CommandPtr::UnwrapVector(std::move(commands)))
+      .ToPtr();
+}
+
+CommandPtr cmd::Race(std::vector<CommandPtr>&& commands) {
+  return ParallelRaceGroup(CommandPtr::UnwrapVector(std::move(commands)))
+      .ToPtr();
+}
+
+CommandPtr cmd::Deadline(CommandPtr&& deadline,
+                         std::vector<CommandPtr>&& others) {
+  return ParallelDeadlineGroup(std::move(deadline).Unwrap(),
+                               CommandPtr::UnwrapVector(std::move(others)))
+      .ToPtr();
+}

commandsv2/src/main/native/cpp/frc2/command/ConditionalCommand.cpp

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/ConditionalCommand.h"
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                                       std::unique_ptr<Command>&& onFalse,
+                                       std::function<bool()> condition)
+    : m_condition{std::move(condition)} {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+      {onTrue.get(), onFalse.get()});
+
+  m_onTrue = std::move(onTrue);
+  m_onFalse = std::move(onFalse);
+
+  m_onTrue->SetComposed(true);
+  m_onFalse->SetComposed(true);
+
+  m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
+  m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
+
+  AddRequirements(m_onTrue->GetRequirements());
+  AddRequirements(m_onFalse->GetRequirements());
+}
+
+void ConditionalCommand::Initialize() {
+  if (m_condition()) {
+    m_selectedCommand = m_onTrue.get();
+  } else {
+    m_selectedCommand = m_onFalse.get();
+  }
+  m_selectedCommand->Initialize();
+}
+
+void ConditionalCommand::Execute() {
+  m_selectedCommand->Execute();
+}
+
+void ConditionalCommand::End(bool interrupted) {
+  m_selectedCommand->End(interrupted);
+}
+
+bool ConditionalCommand::IsFinished() {
+  return m_selectedCommand->IsFinished();
+}
+
+bool ConditionalCommand::RunsWhenDisabled() const {
+  return m_runsWhenDisabled;
+}
+
+Command::InterruptionBehavior ConditionalCommand::GetInterruptionBehavior()
+    const {
+  if (m_onTrue->GetInterruptionBehavior() ==
+          InterruptionBehavior::kCancelSelf ||
+      m_onFalse->GetInterruptionBehavior() ==
+          InterruptionBehavior::kCancelSelf) {
+    return InterruptionBehavior::kCancelSelf;
+  } else {
+    return InterruptionBehavior::kCancelIncoming;
+  }
+}
+
+void ConditionalCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddStringProperty(
+      "onTrue", [this] { return m_onTrue->GetName(); }, nullptr);
+  builder.AddStringProperty(
+      "onFalse", [this] { return m_onFalse->GetName(); }, nullptr);
+  builder.AddStringProperty(
+      "selected",
+      [this] {
+        if (m_selectedCommand) {
+          return m_selectedCommand->GetName();
+        } else {
+          return std::string{"null"};
+        }
+      },
+      nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/DeferredCommand.cpp

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/DeferredCommand.h"
+
+#include <utility>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc2/command/Commands.h"
+
+using namespace frc2;
+
+DeferredCommand::DeferredCommand(wpi::unique_function<CommandPtr()> supplier,
+                                 Requirements requirements)
+    : m_supplier{std::move(supplier)} {
+  AddRequirements(requirements);
+}
+
+void DeferredCommand::Initialize() {
+  m_command = m_supplier().Unwrap();
+  CommandScheduler::GetInstance().RequireUngrouped(m_command.get());
+  m_command->SetComposed(true);
+  m_command->Initialize();
+}
+
+void DeferredCommand::Execute() {
+  m_command->Execute();
+}
+
+void DeferredCommand::End(bool interrupted) {
+  m_command->End(interrupted);
+  m_command =
+      cmd::Print("[DeferredCommand] Lifecycle function called out-of-order!")
+          .WithName("none")
+          .Unwrap();
+}
+
+bool DeferredCommand::IsFinished() {
+  return m_command->IsFinished();
+}
+
+void DeferredCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddStringProperty(
+      "deferred", [this] { return m_command->GetName(); }, nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/FunctionalCommand.cpp

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/FunctionalCommand.h"
+
+#include <utility>
+
+using namespace frc2;
+
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+                                     std::function<void()> onExecute,
+                                     std::function<void(bool)> onEnd,
+                                     std::function<bool()> isFinished,
+                                     Requirements requirements)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {
+  AddRequirements(requirements);
+}
+
+void FunctionalCommand::Initialize() {
+  m_onInit();
+}
+
+void FunctionalCommand::Execute() {
+  m_onExecute();
+}
+
+void FunctionalCommand::End(bool interrupted) {
+  m_onEnd(interrupted);
+}
+
+bool FunctionalCommand::IsFinished() {
+  return m_isFinished();
+}

commandsv2/src/main/native/cpp/frc2/command/InstantCommand.cpp

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/InstantCommand.h"
+
+#include <utility>
+
+using namespace frc2;
+
+InstantCommand::InstantCommand(std::function<void()> toRun,
+                               Requirements requirements)
+    : CommandHelper(
+          std::move(toRun), [] {}, [](bool interrupted) {}, [] { return true; },
+          requirements) {}
+
+InstantCommand::InstantCommand() : InstantCommand([] {}) {}

commandsv2/src/main/native/cpp/frc2/command/NotifierCommand.cpp

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/NotifierCommand.h"
+
+#include <utility>
+
+using namespace frc2;
+
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+                                 units::second_t period,
+                                 Requirements requirements)
+    : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+  AddRequirements(requirements);
+}
+
+NotifierCommand::NotifierCommand(NotifierCommand&& other)
+    : CommandHelper(std::move(other)),
+      m_toRun(other.m_toRun),
+      m_notifier(other.m_toRun),
+      m_period(other.m_period) {}
+
+NotifierCommand::NotifierCommand(const NotifierCommand& other)
+    : CommandHelper(other),
+      m_toRun(other.m_toRun),
+      m_notifier(frc::Notifier(other.m_toRun)),
+      m_period(other.m_period) {}
+
+void NotifierCommand::Initialize() {
+  m_notifier.StartPeriodic(m_period);
+}
+
+void NotifierCommand::End(bool interrupted) {
+  m_notifier.Stop();
+}

commandsv2/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp

@@ -0,0 +1,93 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/ParallelCommandGroup.h"
+
+#include <utility>
+#include <vector>
+
+using namespace frc2;
+
+ParallelCommandGroup::ParallelCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelCommandGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  isRunning = true;
+}
+
+void ParallelCommandGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) {
+      continue;
+    }
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+    }
+  }
+}
+
+void ParallelCommandGroup::End(bool interrupted) {
+  if (interrupted) {
+    for (auto& commandRunning : m_commands) {
+      if (commandRunning.second) {
+        commandRunning.first->End(true);
+      }
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelCommandGroup::IsFinished() {
+  for (auto& command : m_commands) {
+    if (command.second) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool ParallelCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+Command::InterruptionBehavior ParallelCommandGroup::GetInterruptionBehavior()
+    const {
+  return m_interruptBehavior;
+}
+
+void ParallelCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
+
+  if (isRunning) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetComposed(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      if (command->GetInterruptionBehavior() ==
+          Command::InterruptionBehavior::kCancelSelf) {
+        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+      }
+      m_commands.emplace_back(std::move(command), false);
+    } else {
+      throw FRC_MakeError(frc::err::CommandIllegalUse,
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
+    }
+  }
+}

commandsv2/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp

@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/ParallelDeadlineGroup.h"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+ParallelDeadlineGroup::ParallelDeadlineGroup(
+    std::unique_ptr<Command>&& deadline,
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  SetDeadline(std::move(deadline));
+  AddCommands(std::move(commands));
+}
+
+void ParallelDeadlineGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  m_finished = false;
+}
+
+void ParallelDeadlineGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) {
+      continue;
+    }
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+      if (commandRunning.first.get() == m_deadline) {
+        m_finished = true;
+      }
+    }
+  }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    if (commandRunning.second) {
+      commandRunning.first->End(true);
+    }
+  }
+}
+
+bool ParallelDeadlineGroup::IsFinished() {
+  return m_finished;
+}
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+Command::InterruptionBehavior ParallelDeadlineGroup::GetInterruptionBehavior()
+    const {
+  return m_interruptBehavior;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
+
+  if (!m_finished) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetComposed(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      if (command->GetInterruptionBehavior() ==
+          Command::InterruptionBehavior::kCancelSelf) {
+        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+      }
+      m_commands.emplace_back(std::move(command), false);
+    } else {
+      throw FRC_MakeError(frc::err::CommandIllegalUse,
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
+    }
+  }
+}
+
+void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
+  m_deadline = deadline.get();
+  m_deadline->SetComposed(true);
+  m_commands.emplace_back(std::move(deadline), false);
+  AddRequirements(m_deadline->GetRequirements());
+  m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}
+
+void ParallelDeadlineGroup::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+
+  builder.AddStringProperty(
+      "deadline", [this] { return m_deadline->GetName(); }, nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -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.
+
+#include "frc2/command/ParallelRaceGroup.h"
+
+#include <utility>
+#include <vector>
+
+using namespace frc2;
+
+ParallelRaceGroup::ParallelRaceGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelRaceGroup::Initialize() {
+  m_finished = false;
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Initialize();
+  }
+  isRunning = true;
+}
+
+void ParallelRaceGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Execute();
+    if (commandRunning->IsFinished()) {
+      m_finished = true;
+    }
+  }
+}
+
+void ParallelRaceGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->End(!commandRunning->IsFinished());
+  }
+  isRunning = false;
+}
+
+bool ParallelRaceGroup::IsFinished() {
+  return m_finished;
+}
+
+bool ParallelRaceGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+Command::InterruptionBehavior ParallelRaceGroup::GetInterruptionBehavior()
+    const {
+  return m_interruptBehavior;
+}
+
+void ParallelRaceGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
+
+  if (isRunning) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetComposed(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      if (command->GetInterruptionBehavior() ==
+          Command::InterruptionBehavior::kCancelSelf) {
+        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+      }
+      m_commands.emplace_back(std::move(command));
+    } else {
+      throw FRC_MakeError(frc::err::CommandIllegalUse,
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
+    }
+  }
+}

commandsv2/src/main/native/cpp/frc2/command/PrintCommand.cpp

@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/PrintCommand.h"
+
+#include <string>
+
+#include <wpi/print.h>
+
+using namespace frc2;
+
+PrintCommand::PrintCommand(std::string_view message)
+    : CommandHelper{[str = std::string(message)] { wpi::print("{}\n", str); },
+                    {}} {}
+
+bool PrintCommand::RunsWhenDisabled() const {
+  return true;
+}

commandsv2/src/main/native/cpp/frc2/command/ProxyCommand.cpp

@@ -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.
+
+#include "frc2/command/ProxyCommand.h"
+
+#include <string>
+#include <utility>
+
+#include <fmt/format.h>
+#include <wpi/deprecated.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+WPI_IGNORE_DEPRECATED
+ProxyCommand::ProxyCommand(wpi::unique_function<Command*()> supplier)
+    : m_supplier(std::move(supplier)) {}
+
+ProxyCommand::ProxyCommand(wpi::unique_function<CommandPtr()> supplier)
+    : ProxyCommand([supplier = std::move(supplier),
+                    holder = std::optional<CommandPtr>{}]() mutable {
+        holder = supplier();
+        return holder->get();
+      }) {}
+WPI_UNIGNORE_DEPRECATED
+
+ProxyCommand::ProxyCommand(Command* command)
+    : m_supplier([command] { return command; }) {
+  SetName(fmt::format("Proxy({})", command->GetName()));
+}
+
+ProxyCommand::ProxyCommand(std::unique_ptr<Command> command) {
+  SetName(fmt::format("Proxy({})", command->GetName()));
+  m_supplier = [command = std::move(command)] { return command.get(); };
+}
+
+void ProxyCommand::Initialize() {
+  m_command = m_supplier();
+  frc2::CommandScheduler::GetInstance().Schedule(m_command);
+}
+
+void ProxyCommand::End(bool interrupted) {
+  if (interrupted) {
+    m_command->Cancel();
+  }
+  m_command = nullptr;
+}
+
+bool ProxyCommand::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 == nullptr || !m_command->IsScheduled();
+}
+
+void ProxyCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddStringProperty(
+      "proxied",
+      [this] {
+        if (m_command) {
+          return m_command->GetName();
+        } else {
+          return std::string{"null"};
+        }
+      },
+      nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/RepeatCommand.cpp

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/RepeatCommand.h"
+
+#include <string>
+#include <utility>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
+  m_command = std::move(command);
+  m_command->SetComposed(true);
+  AddRequirements(m_command->GetRequirements());
+  SetName(std::string{"Repeat("}.append(m_command->GetName()).append(")"));
+}
+
+void RepeatCommand::Initialize() {
+  m_ended = false;
+  m_command->Initialize();
+}
+
+void RepeatCommand::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;
+  }
+}
+
+bool RepeatCommand::IsFinished() {
+  return false;
+}
+
+void RepeatCommand::End(bool 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;
+  }
+}
+
+bool RepeatCommand::RunsWhenDisabled() const {
+  return m_command->RunsWhenDisabled();
+}
+
+Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const {
+  return m_command->GetInterruptionBehavior();
+}
+
+void RepeatCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddStringProperty(
+      "command", [this] { return m_command->GetName(); }, nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/RunCommand.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/RunCommand.h"
+
+#include <utility>
+
+using namespace frc2;
+
+RunCommand::RunCommand(std::function<void()> toRun, Requirements requirements)
+    : CommandHelper([] {}, std::move(toRun), [](bool interrupted) {},
+                    [] { return false; }, requirements) {}

commandsv2/src/main/native/cpp/frc2/command/ScheduleCommand.cpp

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+
+ScheduleCommand::ScheduleCommand(std::span<Command* const> toSchedule) {
+  for (auto cmd : toSchedule) {
+    m_toSchedule.insert(cmd);
+  }
+}
+
+ScheduleCommand::ScheduleCommand(Command* toSchedule) {
+  m_toSchedule.insert(toSchedule);
+}
+
+void ScheduleCommand::Initialize() {
+  for (auto command : m_toSchedule) {
+    frc2::CommandScheduler::GetInstance().Schedule(command);
+  }
+}
+
+bool ScheduleCommand::IsFinished() {
+  return true;
+}
+
+bool ScheduleCommand::RunsWhenDisabled() const {
+  return true;
+}

commandsv2/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp

@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/SequentialCommandGroup.h"
+
+#include <utility>
+#include <vector>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+SequentialCommandGroup::SequentialCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void SequentialCommandGroup::Initialize() {
+  m_currentCommandIndex = 0;
+
+  if (!m_commands.empty()) {
+    m_commands[0]->Initialize();
+  }
+}
+
+void SequentialCommandGroup::Execute() {
+  if (m_commands.empty()) {
+    return;
+  }
+
+  auto& currentCommand = m_commands[m_currentCommandIndex];
+
+  currentCommand->Execute();
+  if (currentCommand->IsFinished()) {
+    currentCommand->End(false);
+    m_currentCommandIndex++;
+    if (m_currentCommandIndex < m_commands.size()) {
+      m_commands[m_currentCommandIndex]->Initialize();
+    }
+  }
+}
+
+void SequentialCommandGroup::End(bool interrupted) {
+  if (interrupted && !m_commands.empty() &&
+      m_currentCommandIndex != invalid_index &&
+      m_currentCommandIndex < m_commands.size()) {
+    m_commands[m_currentCommandIndex]->End(interrupted);
+  }
+  m_currentCommandIndex = invalid_index;
+}
+
+bool SequentialCommandGroup::IsFinished() {
+  return m_currentCommandIndex == m_commands.size();
+}
+
+bool SequentialCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+Command::InterruptionBehavior SequentialCommandGroup::GetInterruptionBehavior()
+    const {
+  return m_interruptBehavior;
+}
+
+void SequentialCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
+
+  if (m_currentCommandIndex != invalid_index) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    command->SetComposed(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    if (command->GetInterruptionBehavior() ==
+        Command::InterruptionBehavior::kCancelSelf) {
+      m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+    }
+    m_commands.emplace_back(std::move(command));
+  }
+}
+
+void SequentialCommandGroup::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddIntegerProperty(
+      "index", [this] { return m_currentCommandIndex; }, nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/StartEndCommand.cpp

@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/StartEndCommand.h"
+
+#include <utility>
+
+using namespace frc2;
+
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+                                 std::function<void()> onEnd,
+                                 Requirements requirements)
+    : CommandHelper(
+          std::move(onInit), [] {},
+          [onEnd = std::move(onEnd)](bool interrupted) { onEnd(); },
+          [] { return false; }, requirements) {}

commandsv2/src/main/native/cpp/frc2/command/Subsystem.cpp

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/Subsystem.h"
+
+#include <string>
+#include <utility>
+
+#include <wpi/Demangle.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Commands.h"
+
+using namespace frc2;
+Subsystem::~Subsystem() {
+  CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
+void Subsystem::Periodic() {}
+
+void Subsystem::SimulationPeriodic() {}
+
+std::string Subsystem::GetName() const {
+  return wpi::GetTypeName(*this);
+}
+
+void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
+  CommandScheduler::GetInstance().SetDefaultCommand(this,
+                                                    std::move(defaultCommand));
+}
+
+void Subsystem::RemoveDefaultCommand() {
+  CommandScheduler::GetInstance().RemoveDefaultCommand(this);
+}
+
+Command* Subsystem::GetDefaultCommand() const {
+  return CommandScheduler::GetInstance().GetDefaultCommand(this);
+}
+
+Command* Subsystem::GetCurrentCommand() const {
+  return CommandScheduler::GetInstance().Requiring(this);
+}
+
+void Subsystem::Register() {
+  return CommandScheduler::GetInstance().RegisterSubsystem(this);
+}
+
+CommandPtr Subsystem::Idle() {
+  return cmd::Idle({this});
+}
+
+CommandPtr Subsystem::RunOnce(std::function<void()> action) {
+  return cmd::RunOnce(std::move(action), {this});
+}
+
+CommandPtr Subsystem::Run(std::function<void()> action) {
+  return cmd::Run(std::move(action), {this});
+}
+
+CommandPtr Subsystem::StartEnd(std::function<void()> start,
+                               std::function<void()> end) {
+  return cmd::StartEnd(std::move(start), std::move(end), {this});
+}
+
+CommandPtr Subsystem::RunEnd(std::function<void()> run,
+                             std::function<void()> end) {
+  return cmd::RunEnd(std::move(run), std::move(end), {this});
+}
+
+CommandPtr Subsystem::StartRun(std::function<void()> start,
+                               std::function<void()> run) {
+  return cmd::StartRun(std::move(start), std::move(run), {this});
+}
+
+CommandPtr Subsystem::Defer(wpi::unique_function<CommandPtr()> supplier) {
+  return cmd::Defer(std::move(supplier), {this});
+}

commandsv2/src/main/native/cpp/frc2/command/SubsystemBase.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/SubsystemBase.h"
+
+#include <string>
+
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
+using namespace frc2;
+
+SubsystemBase::SubsystemBase() {
+  wpi::SendableRegistry::Add(this, GetTypeName(*this));
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+SubsystemBase::SubsystemBase(std::string_view name) {
+  wpi::SendableRegistry::Add(this, name);
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+  builder.AddBooleanProperty(
+      ".hasDefault", [this] { return GetDefaultCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".default",
+      [this]() -> std::string {
+        auto command = GetDefaultCommand();
+        if (command == nullptr) {
+          return "none";
+        }
+        return command->GetName();
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      ".hasCommand", [this] { return GetCurrentCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".command",
+      [this]() -> std::string {
+        auto command = GetCurrentCommand();
+        if (command == nullptr) {
+          return "none";
+        }
+        return command->GetName();
+      },
+      nullptr);
+}
+
+std::string SubsystemBase::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+void SubsystemBase::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
+}
+
+std::string SubsystemBase::GetSubsystem() const {
+  return wpi::SendableRegistry::GetSubsystem(this);
+}
+
+void SubsystemBase::SetSubsystem(std::string_view name) {
+  wpi::SendableRegistry::SetSubsystem(this, name);
+}
+
+void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) {
+  wpi::SendableRegistry::Add(child, GetSubsystem(), name);
+}

commandsv2/src/main/native/cpp/frc2/command/WaitCommand.cpp

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/WaitCommand.h"
+
+#include <fmt/format.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+using namespace frc2;
+
+WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
+  SetName(fmt::format("{}: {}", GetName(), duration));
+}
+
+void WaitCommand::Initialize() {
+  m_timer.Restart();
+}
+
+void WaitCommand::End(bool interrupted) {
+  m_timer.Stop();
+}
+
+bool WaitCommand::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}
+
+bool WaitCommand::RunsWhenDisabled() const {
+  return true;
+}
+
+void WaitCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddDoubleProperty(
+      "duration", [this] { return m_duration.value(); }, nullptr);
+}

commandsv2/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/WaitUntilCommand.h"
+
+#include <utility>
+
+#include <frc/Timer.h>
+
+using namespace frc2;
+
+WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
+    : m_condition{std::move(condition)} {}
+
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0_s; }} {}
+
+bool WaitUntilCommand::IsFinished() {
+  return m_condition();
+}
+
+bool WaitUntilCommand::RunsWhenDisabled() const {
+  return true;
+}

commandsv2/src/main/native/cpp/frc2/command/WrapperCommand.cpp

@@ -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.
+
+#include "frc2/command/WrapperCommand.h"
+
+#include <utility>
+
+#include "frc2/command/Command.h"
+
+using namespace frc2;
+
+WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
+  CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
+  m_command = std::move(command);
+  m_command->SetComposed(true);
+  // copy the wrapped command's name
+  SetName(m_command->GetName());
+}
+
+void WrapperCommand::Initialize() {
+  m_command->Initialize();
+}
+
+void WrapperCommand::Execute() {
+  m_command->Execute();
+}
+
+bool WrapperCommand::IsFinished() {
+  return m_command->IsFinished();
+}
+
+void WrapperCommand::End(bool interrupted) {
+  m_command->End(interrupted);
+}
+
+bool WrapperCommand::RunsWhenDisabled() const {
+  return m_command->RunsWhenDisabled();
+}
+
+Command::InterruptionBehavior WrapperCommand::GetInterruptionBehavior() const {
+  return m_command->GetInterruptionBehavior();
+}
+
+wpi::SmallSet<Subsystem*, 4> WrapperCommand::GetRequirements() const {
+  return m_command->GetRequirements();
+}

commandsv2/src/main/native/cpp/frc2/command/button/CommandGamepad.cpp

@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/CommandGamepad.h"
+
+using namespace frc2;
+
+CommandGamepad::CommandGamepad(int port)
+    : CommandGenericHID(port), m_hid{frc::Gamepad(port)} {}
+
+frc::Gamepad& CommandGamepad::GetHID() {
+  return m_hid;
+}
+
+Trigger CommandGamepad::SouthFace(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kSouthFace, loop);
+}
+
+Trigger CommandGamepad::EastFace(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kEastFace, loop);
+}
+
+Trigger CommandGamepad::WestFace(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kWestFace, loop);
+}
+
+Trigger CommandGamepad::NorthFace(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kNorthFace, loop);
+}
+
+Trigger CommandGamepad::Back(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kBack, loop);
+}
+
+Trigger CommandGamepad::Guide(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kGuide, loop);
+}
+
+Trigger CommandGamepad::Start(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kStart, loop);
+}
+
+Trigger CommandGamepad::LeftStick(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kLeftStick, loop);
+}
+
+Trigger CommandGamepad::RightStick(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kRightStick, loop);
+}
+
+Trigger CommandGamepad::LeftShoulder(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kLeftShoulder, loop);
+}
+
+Trigger CommandGamepad::RightShoulder(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kRightShoulder, loop);
+}
+
+Trigger CommandGamepad::DpadUp(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kDpadUp, loop);
+}
+
+Trigger CommandGamepad::DpadDown(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kDpadDown, loop);
+}
+
+Trigger CommandGamepad::DpadLeft(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kDpadLeft, loop);
+}
+
+Trigger CommandGamepad::DpadRight(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kDpadRight, loop);
+}
+
+Trigger CommandGamepad::Misc1(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc1, loop);
+}
+
+Trigger CommandGamepad::RightPaddle1(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kRightPaddle1, loop);
+}
+
+Trigger CommandGamepad::LeftPaddle1(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kLeftPaddle1, loop);
+}
+
+Trigger CommandGamepad::RightPaddle2(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kRightPaddle2, loop);
+}
+
+Trigger CommandGamepad::LeftPaddle2(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kLeftPaddle2, loop);
+}
+
+Trigger CommandGamepad::Touchpad(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kTouchpad, loop);
+}
+
+Trigger CommandGamepad::Misc2(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc2, loop);
+}
+
+Trigger CommandGamepad::Misc3(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc3, loop);
+}
+
+Trigger CommandGamepad::Misc4(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc4, loop);
+}
+
+Trigger CommandGamepad::Misc5(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc5, loop);
+}
+
+Trigger CommandGamepad::Misc6(frc::EventLoop* loop) const {
+  return Button(frc::Gamepad::Button::kMisc6, loop);
+}
+
+Trigger CommandGamepad::LeftTrigger(double threshold,
+                                    frc::EventLoop* loop) const {
+  return Trigger(loop, [this, threshold] {
+    return m_hid.GetLeftTriggerAxis() > threshold;
+  });
+}
+
+Trigger CommandGamepad::RightTrigger(double threshold,
+                                     frc::EventLoop* loop) const {
+  return Trigger(loop, [this, threshold] {
+    return m_hid.GetRightTriggerAxis() > threshold;
+  });
+}
+
+double CommandGamepad::GetLeftX() const {
+  return m_hid.GetLeftX();
+}
+
+double CommandGamepad::GetLeftY() const {
+  return m_hid.GetLeftY();
+}
+
+double CommandGamepad::GetRightX() const {
+  return m_hid.GetRightX();
+}
+
+double CommandGamepad::GetRightY() const {
+  return m_hid.GetRightY();
+}
+
+double CommandGamepad::GetLeftTriggerAxis() const {
+  return m_hid.GetLeftTriggerAxis();
+}
+
+double CommandGamepad::GetRightTriggerAxis() const {
+  return m_hid.GetRightTriggerAxis();
+}

commandsv2/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/CommandGenericHID.h"
+
+using namespace frc2;
+
+CommandGenericHID::CommandGenericHID(int port) : m_hid{port} {}
+
+frc::GenericHID& CommandGenericHID::GetHID() {
+  return m_hid;
+}
+
+Trigger CommandGenericHID::Button(int button, frc::EventLoop* loop) const {
+  return Trigger(loop, [this, button] { return m_hid.GetRawButton(button); });
+}
+
+Trigger CommandGenericHID::POV(frc::DriverStation::POVDirection angle,
+                               frc::EventLoop* loop) const {
+  return POV(0, angle, loop);
+}
+
+Trigger CommandGenericHID::POV(int pov, frc::DriverStation::POVDirection angle,
+                               frc::EventLoop* loop) const {
+  return Trigger(loop,
+                 [this, pov, angle] { return m_hid.GetPOV(pov) == angle; });
+}
+
+Trigger CommandGenericHID::POVUp(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kUp, loop);
+}
+
+Trigger CommandGenericHID::POVUpRight(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kUpRight, loop);
+}
+
+Trigger CommandGenericHID::POVRight(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kRight, loop);
+}
+
+Trigger CommandGenericHID::POVDownRight(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kDownRight, loop);
+}
+
+Trigger CommandGenericHID::POVDown(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kDown, loop);
+}
+
+Trigger CommandGenericHID::POVDownLeft(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kDownLeft, loop);
+}
+
+Trigger CommandGenericHID::POVLeft(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kLeft, loop);
+}
+
+Trigger CommandGenericHID::POVUpLeft(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kUpLeft, loop);
+}
+
+Trigger CommandGenericHID::POVCenter(frc::EventLoop* loop) const {
+  return POV(frc::DriverStation::POVDirection::kCenter, loop);
+}
+
+Trigger CommandGenericHID::AxisLessThan(int axis, double threshold,
+                                        frc::EventLoop* loop) const {
+  return Trigger(loop, [this, axis, threshold]() {
+    return m_hid.GetRawAxis(axis) < threshold;
+  });
+}
+
+Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold,
+                                           frc::EventLoop* loop) const {
+  return Trigger(loop, [this, axis, threshold]() {
+    return m_hid.GetRawAxis(axis) > threshold;
+  });
+}
+
+Trigger CommandGenericHID::AxisMagnitudeGreaterThan(
+    int axis, double threshold, frc::EventLoop* loop) const {
+  return Trigger(loop, [this, axis, threshold]() {
+    return std::abs(m_hid.GetRawAxis(axis)) > threshold;
+  });
+}
+
+void CommandGenericHID::SetRumble(frc::GenericHID::RumbleType type,
+                                  double value) {
+  m_hid.SetRumble(type, value);
+}
+
+bool CommandGenericHID::IsConnected() const {
+  return m_hid.IsConnected();
+}

commandsv2/src/main/native/cpp/frc2/command/button/CommandJoystick.cpp

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/CommandJoystick.h"
+
+using namespace frc2;
+
+CommandJoystick::CommandJoystick(int port)
+    : CommandGenericHID(port), m_hid{frc::Joystick(port)} {}
+
+frc::Joystick& CommandJoystick::GetHID() {
+  return m_hid;
+}
+
+Trigger CommandJoystick::Trigger(frc::EventLoop* loop) const {
+  return Button(frc::Joystick::ButtonType::kTriggerButton, loop);
+}
+
+Trigger CommandJoystick::Top(frc::EventLoop* loop) const {
+  return Button(frc::Joystick::ButtonType::kTopButton, loop);
+}
+
+double CommandJoystick::GetMagnitude() const {
+  return m_hid.GetMagnitude();
+}
+
+units::radian_t CommandJoystick::GetDirection() const {
+  // 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.GetDirection();
+}

commandsv2/src/main/native/cpp/frc2/command/button/NetworkButton.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/NetworkButton.h"
+
+#include <memory>
+#include <utility>
+
+using namespace frc2;
+
+NetworkButton::NetworkButton(nt::BooleanTopic topic)
+    : NetworkButton(topic.Subscribe(false)) {}
+
+NetworkButton::NetworkButton(nt::BooleanSubscriber sub)
+    : Trigger([sub = std::make_shared<nt::BooleanSubscriber>(std::move(sub))] {
+        return sub->GetTopic().GetInstance().IsConnected() && sub->Get();
+      }) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                             std::string_view field)
+    : NetworkButton(table->GetBooleanTopic(field)) {}
+
+NetworkButton::NetworkButton(std::string_view table, std::string_view field)
+    : NetworkButton(nt::NetworkTableInstance::GetDefault(), table, field) {}
+
+NetworkButton::NetworkButton(nt::NetworkTableInstance inst,
+                             std::string_view table, std::string_view field)
+    : NetworkButton(inst.GetTable(table), field) {}

commandsv2/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/RobotModeTriggers.h"
+
+#include <frc/DriverStation.h>
+
+using namespace frc2;
+
+Trigger RobotModeTriggers::Autonomous() {
+  return Trigger{&frc::DriverStation::IsAutonomousEnabled};
+}
+
+Trigger RobotModeTriggers::Teleop() {
+  return Trigger{&frc::DriverStation::IsTeleopEnabled};
+}
+
+Trigger RobotModeTriggers::Disabled() {
+  return Trigger{&frc::DriverStation::IsDisabled};
+}
+
+Trigger RobotModeTriggers::Test() {
+  return Trigger{&frc::DriverStation::IsTestEnabled};
+}

commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -0,0 +1,189 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/Trigger.h"
+
+#include <utility>
+
+#include <frc/filter/Debouncer.h>
+
+#include "frc2/command/CommandPtr.h"
+
+using namespace frc;
+using namespace frc2;
+
+Trigger::Trigger(const Trigger& other) = default;
+
+void Trigger::AddBinding(wpi::unique_function<void(bool, bool)>&& body) {
+  m_loop->Bind([condition = m_condition, previous = m_condition(),
+                body = std::move(body)]() mutable {
+    bool current = condition();
+
+    body(previous, current);
+
+    previous = current;
+  });
+}
+
+Trigger Trigger::OnChange(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (previous != current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::OnChange(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (previous != current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::OnTrue(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (!previous && current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::OnTrue(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (!previous && current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::OnFalse(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (previous && !current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::OnFalse(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (previous && !current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::WhileTrue(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (!previous && current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    } else if (previous && !current) {
+      command->Cancel();
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::WhileTrue(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (!previous && current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    } else if (previous && !current) {
+      command.Cancel();
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::WhileFalse(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (previous && !current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    } else if (!previous && current) {
+      command->Cancel();
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::WhileFalse(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (!previous && current) {
+      frc2::CommandScheduler::GetInstance().Schedule(command);
+    } else if (previous && !current) {
+      command.Cancel();
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::ToggleOnTrue(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (!previous && current) {
+      if (command->IsScheduled()) {
+        command->Cancel();
+      } else {
+        frc2::CommandScheduler::GetInstance().Schedule(command);
+      }
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::ToggleOnTrue(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (!previous && current) {
+      if (command.IsScheduled()) {
+        command.Cancel();
+      } else {
+        frc2::CommandScheduler::GetInstance().Schedule(command);
+      }
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::ToggleOnFalse(Command* command) {
+  AddBinding([command](bool previous, bool current) {
+    if (previous && !current) {
+      if (command->IsScheduled()) {
+        command->Cancel();
+      } else {
+        frc2::CommandScheduler::GetInstance().Schedule(command);
+      }
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::ToggleOnFalse(CommandPtr&& command) {
+  AddBinding([command = std::move(command)](bool previous, bool current) {
+    if (previous && !current) {
+      if (command.IsScheduled()) {
+        command.Cancel();
+      } else {
+        frc2::CommandScheduler::GetInstance().Schedule(command);
+      }
+    }
+  });
+  return *this;
+}
+
+Trigger Trigger::Debounce(units::second_t debounceTime,
+                          frc::Debouncer::DebounceType type) {
+  return Trigger(m_loop, [debouncer = frc::Debouncer(debounceTime, type),
+                          condition = m_condition]() mutable {
+    return debouncer.Calculate(condition());
+  });
+}
+
+bool Trigger::Get() const {
+  return m_condition();
+}

commandsv2/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/sysid/SysIdRoutine.h"
+
+#include <frc/sysid/SysIdRoutineLog.h>
+
+using namespace frc2::sysid;
+
+frc2::CommandPtr SysIdRoutine::Quasistatic(Direction direction) {
+  frc::sysid::State state;
+  if (direction == Direction::kForward) {
+    state = frc::sysid::State::kQuasistaticForward;
+  } else {  // if (direction == Direction::kReverse) {
+    state = frc::sysid::State::kQuasistaticReverse;
+  }
+
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem->RunOnce([this] { timer.Restart(); })
+      .AndThen(
+          m_mechanism.m_subsystem
+              ->Run([this, state, outputSign] {
+                m_outputVolts = outputSign * timer.Get() * m_config.m_rampRate;
+                m_mechanism.m_drive(m_outputVolts);
+                m_mechanism.m_log(this);
+                m_recordState(state);
+              })
+              .FinallyDo([this] {
+                m_mechanism.m_drive(0_V);
+                m_recordState(frc::sysid::State::kNone);
+                timer.Stop();
+              })
+              .WithName("sysid-" +
+                        frc::sysid::SysIdRoutineLog::StateEnumToString(state) +
+                        "-" + m_mechanism.m_name)
+              .WithTimeout(m_config.m_timeout));
+}
+
+frc2::CommandPtr SysIdRoutine::Dynamic(Direction direction) {
+  frc::sysid::State state;
+  if (direction == Direction::kForward) {
+    state = frc::sysid::State::kDynamicForward;
+  } else {  // if (direction == Direction::kReverse) {
+    state = frc::sysid::State::kDynamicReverse;
+  }
+
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem
+      ->RunOnce([this] { m_outputVolts = m_config.m_stepVoltage; })
+      .AndThen(m_mechanism.m_subsystem->Run([this, state, outputSign] {
+        m_mechanism.m_drive(m_outputVolts * outputSign);
+        m_mechanism.m_log(this);
+        m_recordState(state);
+      }))
+      .FinallyDo([this] {
+        m_mechanism.m_drive(0_V);
+        m_recordState(frc::sysid::State::kNone);
+      })
+      .WithName("sysid-" +
+                frc::sysid::SysIdRoutineLog::StateEnumToString(state) + "-" +
+                m_mechanism.m_name)
+      .WithTimeout(m_config.m_timeout);
+}

commandsv2/src/main/native/include/frc2/command/Command.h

@@ -0,0 +1,497 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <units/time.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/Sendable.h>
+
+#include "frc2/command/Requirements.h"
+#include "frc2/command/Subsystem.h"
+
+namespace frc2 {
+
+/**
+ * A state machine representing a complete action to be performed by the robot.
+ * Commands are run by the CommandScheduler, and can be composed into
+ * CommandGroups to allow users to build complicated multi-step 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>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * This class is provided by the NewCommands VendorDep
+ *
+ * @see CommandScheduler
+ * @see CommandHelper
+ */
+class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
+ public:
+  ~Command() override;
+
+  Command(const Command&) = default;
+  Command& operator=(const Command& rhs);
+  Command(Command&&) = default;
+  Command& operator=(Command&&) = default;
+
+  /**
+   * The initial subroutine of a command.  Called once when the command is
+   * initially scheduled.
+   */
+  virtual void Initialize();
+
+  /**
+   * The main body of a command.  Called repeatedly while the command is
+   * scheduled.
+   */
+  virtual void Execute();
+
+  /**
+   * The action to take when the command ends.  Called when either the command
+   * finishes normally, or when it interrupted/canceled.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  virtual void End(bool 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.
+   */
+  virtual bool IsFinished() { return false; }
+
+  /**
+   * Specifies the set of subsystems used by this command. Two commands cannot
+   * use the same subsystem at the same time. If another command is scheduled
+   * that shares a requirement, GetInterruptionBehavior() will be checked and
+   * followed. If no subsystems are required, return an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the
+   * requirements as a field, and return that field here, rather than allocating
+   * a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   * @see InterruptionBehavior
+   */
+  virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const;
+
+  /**
+   * Adds the specified Subsystem requirements to the command.
+   *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * 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.
+   *
+   * While this overload can be used with {@code AddRequirements({&subsystem1,
+   * &subsystem2})}, {@code AddRequirements({&subsystem})} selects the {@code
+   * AddRequirements(Subsystem*)} overload, which will function identically but
+   * may cause warnings about redundant braces.
+   *
+   * @param requirements the Subsystem requirements to add, which can be
+   * implicitly constructed from an initializer list or a span
+   */
+  void AddRequirements(Requirements requirements);
+
+  /**
+   * Adds the specified Subsystem requirements to the command.
+   *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * 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 Subsystem requirements to add
+   */
+  void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
+
+  /**
+   * Adds the specified Subsystem requirement to the command.
+   *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * 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 requirement the Subsystem requirement to add
+   */
+  void AddRequirements(Subsystem* requirement);
+
+  /**
+   * Gets the name of this Command.
+   *
+   * @return Name
+   */
+  std::string GetName() const;
+
+  /**
+   * Sets the name of this Command.
+   *
+   * @param name name
+   */
+  void SetName(std::string_view name);
+
+  /**
+   * Gets the subsystem name of this Command.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Command.
+   *
+   * @param subsystem subsystem name
+   */
+  void SetSubsystem(std::string_view subsystem);
+
+  /**
+   * An enum describing the command's behavior when another command with a
+   * shared requirement is scheduled.
+   */
+  enum class InterruptionBehavior {
+    /**
+     * This command ends, End(true) is called, and the incoming command is
+     * scheduled normally.
+     *
+     * <p>This is the default behavior.
+     */
+    kCancelSelf,
+    /** This command continues, and the incoming command is not scheduled. */
+    kCancelIncoming
+  };
+
+  friend class CommandPtr;
+
+  /**
+   * 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.
+   *
+   * @param duration the timeout duration
+   * @return the command with the timeout added
+   */
+  CommandPtr WithTimeout(units::second_t duration) &&;
+
+  /**
+   * 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.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   */
+  CommandPtr Until(std::function<bool()> 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.
+   *
+   * @param condition the run condition
+   * @return the command with the run condition added
+   */
+  CommandPtr OnlyWhile(std::function<bool()> condition) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  CommandPtr BeforeStarting(std::function<void()> toRun,
+                            Requirements requirements = {}) &&;
+
+  /**
+   * Decorates this command with another command to run before this command
+   * starts.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  CommandPtr BeforeStarting(CommandPtr&& before) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  CommandPtr AndThen(std::function<void()> toRun,
+                     Requirements requirements = {}) &&;
+
+  /**
+   * Decorates this command with a set of commands to run after it in sequence.
+   * Often more convenient/less-verbose than constructing a
+   * SequentialCommandGroup explicitly.
+   *
+   * @param next the commands to run next
+   * @return the decorated command
+   */
+  CommandPtr AndThen(CommandPtr&& next) &&;
+
+  /**
+   * Decorates this command to run repeatedly, restarting it when it ends, until
+   * this command is interrupted. The decorated command can still be canceled.
+   *
+   * @return the decorated command
+   */
+  CommandPtr Repeatedly() &&;
+
+  /**
+   * Decorates this command to run "by proxy" by wrapping it in a 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 <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+   * WPILib docs</a> for a full explanation.
+   *
+   * <p>This overload transfers command ownership to the returned CommandPtr.
+   *
+   * @return the decorated command
+   * @see ProxyCommand
+   */
+  CommandPtr AsProxy() &&;
+
+  /**
+   * 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.
+   *
+   * @param condition the condition that will prevent the command from running
+   * @return the decorated command
+   */
+  CommandPtr Unless(std::function<bool()> 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.
+   *
+   * @param condition the condition that will allow the command to run
+   * @return the decorated command
+   */
+  CommandPtr OnlyIf(std::function<bool()> condition) &&;
+
+  /**
+   * Creates a new command that runs this command and the deadline in parallel,
+   * finishing (and interrupting this command) when the deadline finishes.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see DeadlineFor
+   */
+  CommandPtr WithDeadline(CommandPtr&& deadline) &&;
+
+  /**
+   * 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.
+   *
+   * @param parallel the commands to run in parallel. Note the parallel commands
+   * will be interupted when the deadline command ends
+   * @return the decorated command
+   * @see WithDeadline
+   */
+  CommandPtr DeadlineFor(CommandPtr&& 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.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  CommandPtr AlongWith(CommandPtr&& parallel) &&;
+
+  /**
+   * 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.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  CommandPtr RaceWith(CommandPtr&& parallel) &&;
+
+  /**
+   * Decorates this command to run or stop when disabled.
+   *
+   * @param doesRunWhenDisabled true to run when disabled.
+   * @return the decorated command
+   */
+  CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&;
+
+  /**
+   * Decorates this command to have a different interrupt behavior.
+   *
+   * @param interruptBehavior the desired interrupt behavior
+   * @return the decorated command
+   */
+  CommandPtr WithInterruptBehavior(
+      Command::InterruptionBehavior interruptBehavior) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following
+   * the command's inherent Command::End(bool) method.
+   *
+   * @param end a lambda accepting a boolean parameter specifying whether the
+   * command was interrupted.
+   * @return the decorated command
+   */
+  CommandPtr FinallyDo(std::function<void(bool)> end) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following
+   * the command's inherent Command::End(bool) 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
+   */
+  CommandPtr FinallyDo(std::function<void()> end) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt, following the
+   * command's inherent Command::End(bool) method.
+   *
+   * @param handler a lambda to run when the command is interrupted
+   * @return the decorated command
+   */
+  CommandPtr HandleInterrupt(std::function<void()> handler) &&;
+
+  /**
+   * Decorates this Command with a name.
+   *
+   * @param name name
+   * @return the decorated Command
+   */
+  CommandPtr WithName(std::string_view name) &&;
+
+  /**
+   * Schedules this command.
+   *
+   * @deprecated Use CommandScheduler::GetInstance().Schedule() instead
+   */
+  [[deprecated("Use CommandScheduler::GetInstance().Schedule() instead.")]]
+  void Schedule();
+
+  /**
+   * Cancels this command. Will call End(true). Commands will be canceled
+   * regardless of interruption behavior.
+   */
+  void Cancel();
+
+  /**
+   * Whether or not 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.
+   */
+  bool IsScheduled() const;
+
+  /**
+   * Whether the command requires a given subsystem.  Named "HasRequirement"
+   * rather than "requires" to avoid confusion with Command::Requires(Subsystem)
+   * -- this may be able to be changed in a few years.
+   *
+   * @param requirement the subsystem to inquire about
+   * @return whether the subsystem is required
+   */
+  bool HasRequirement(Subsystem* requirement) const;
+
+  /**
+   * Whether the command is currently grouped in a command group.  Used as extra
+   * insurance to prevent accidental independent use of grouped commands.
+   */
+  bool IsComposed() const;
+
+  /**
+   * Sets whether the command is currently composed in a command composition.
+   * Can be used to "reclaim" a command if a composition is no longer going to
+   * use it.  NOT ADVISED!
+   */
+  void SetComposed(bool isComposed);
+
+  /**
+   * Get the stacktrace of where this command was composed, or an empty
+   * optional. Intended for internal use.
+   *
+   * @return optional string representation of the composition site stack trace.
+   */
+  std::optional<std::string> GetPreviousCompositionSite() const;
+
+  /**
+   * 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
+   */
+  virtual bool RunsWhenDisabled() const { return false; }
+
+  /**
+   * How the command behaves when another command with a shared requirement is
+   * scheduled.
+   *
+   * @return a variant of InterruptionBehavior, defaulting to kCancelSelf.
+   */
+  virtual InterruptionBehavior GetInterruptionBehavior() const {
+    return InterruptionBehavior::kCancelSelf;
+  }
+
+  /**
+   * Transfers ownership of this command to a unique pointer.  Used for
+   * decorator methods.
+   */
+  virtual CommandPtr ToPtr() && = 0;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ protected:
+  Command();
+
+ private:
+  /// Requirements set.
+  wpi::SmallSet<Subsystem*, 4> m_requirements;
+
+  std::optional<std::string> m_previousComposition;
+};
+
+/**
+ * Checks if two commands have disjoint requirement sets.
+ *
+ * @param first The first command to check.
+ * @param second The second command to check.
+ * @return False if first and second share a requirement.
+ */
+bool RequirementsDisjoint(Command* first, Command* second);
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/CommandHelper.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandPtr.h"
+
+namespace frc2 {
+
+/**
+ * CRTP implementation to allow polymorphic decorator functions in Command.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+template <std::derived_from<Command> Base, typename CRTP>
+class CommandHelper : public Base {
+  using Base::Base;
+
+ public:
+  CommandHelper() = default;
+
+  CommandPtr ToPtr() && override {
+    return CommandPtr(
+        std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this))));
+  }
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/CommandPtr.h

@@ -0,0 +1,342 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * A wrapper around std::unique_ptr<Command> so commands have move-only
+ * semantics. Commands should only be stored and passed around when held in a
+ * CommandPtr instance. For more info, see
+ * https://github.com/wpilibsuite/allwpilib/issues/4303.
+ *
+ * Various classes in the command-based library accept a
+ * std::unique_ptr<Command>, use CommandPtr::Unwrap to convert.
+ * CommandPtr::UnwrapVector does the same for vectors.
+ */
+class [[nodiscard]] CommandPtr final {
+ public:
+  explicit CommandPtr(std::unique_ptr<Command>&& command);
+
+  template <std::derived_from<Command> T>
+  // NOLINTNEXTLINE(bugprone-forwarding-reference-overload)
+  explicit CommandPtr(T&& command)
+      : CommandPtr(
+            std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
+
+  CommandPtr(CommandPtr&&);
+  CommandPtr& operator=(CommandPtr&&) = default;
+
+  explicit CommandPtr(std::nullptr_t) = delete;
+
+  /**
+   * Decorates this command to run repeatedly, restarting it when it ends, until
+   * this command is interrupted. The decorated command can still be canceled.
+   *
+   * @return the decorated command
+   */
+  CommandPtr Repeatedly() &&;
+
+  /**
+   * Decorates this command to run "by proxy" by wrapping it in a 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 <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+   * WPILib docs</a> for a full explanation.
+   *
+   * @return the decorated command
+   * @see ProxyCommand
+   */
+  CommandPtr AsProxy() &&;
+
+  /**
+   * Decorates this command to run or stop when disabled.
+   *
+   * @param doesRunWhenDisabled true to run when disabled
+   * @return the decorated command
+   */
+  CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&;
+
+  /**
+   * Decorates this command to have a different interrupt behavior.
+   *
+   * @param interruptBehavior the desired interrupt behavior
+   * @return the decorated command
+   */
+  CommandPtr WithInterruptBehavior(
+      Command::InterruptionBehavior interruptBehavior) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  CommandPtr AndThen(std::function<void()> toRun,
+                     Requirements 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.
+   *
+   * @param next the commands to run next
+   * @return the decorated command
+   */
+  CommandPtr AndThen(CommandPtr&& next) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  CommandPtr BeforeStarting(std::function<void()> toRun,
+                            Requirements requirements = {}) &&;
+
+  /**
+   * Decorates this command with another command to run before this command
+   * starts.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  CommandPtr BeforeStarting(CommandPtr&& before) &&;
+
+  /**
+   * 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.
+   *
+   * @param duration the timeout duration
+   * @return the command with the timeout added
+   */
+  CommandPtr WithTimeout(units::second_t duration) &&;
+
+  /**
+   * 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.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   */
+  CommandPtr Until(std::function<bool()> 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.
+   *
+   * @param condition the run condition
+   * @return the command with the run condition added
+   */
+  CommandPtr OnlyWhile(std::function<bool()> condition) &&;
+
+  /**
+   * 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.
+   *
+   * @param condition the condition that will prevent the command from running
+   * @return the decorated command
+   */
+  CommandPtr Unless(std::function<bool()> 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.
+   *
+   * @param condition the condition that will allow the command to run
+   * @return the decorated command
+   */
+  CommandPtr OnlyIf(std::function<bool()> condition) &&;
+
+  /**
+   * Creates a new command that runs this command and the deadline in parallel,
+   * finishing (and interrupting this command) when the deadline finishes.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see DeadlineFor
+   */
+  CommandPtr WithDeadline(CommandPtr&& deadline) &&;
+
+  /**
+   * 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.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  [[deprecated("Replace with DeadlineFor")]]
+  CommandPtr DeadlineWith(CommandPtr&& 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.
+   *
+   * @param parallel the commands to run in parallel. Note the parallel commands
+   * will be interupted when the deadline command ends
+   * @return the decorated command
+   */
+  CommandPtr DeadlineFor(CommandPtr&& 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.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  CommandPtr AlongWith(CommandPtr&& parallel) &&;
+
+  /**
+   * 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.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  CommandPtr RaceWith(CommandPtr&& parallel) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following
+   * the command's inherent Command::End(bool) method.
+   *
+   * @param end a lambda accepting a boolean parameter specifying whether the
+   * command was interrupted
+   * @return the decorated command
+   */
+  CommandPtr FinallyDo(std::function<void(bool)> end) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt or end, following
+   * the command's inherent Command::End(bool) 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
+   */
+  CommandPtr FinallyDo(std::function<void()> end) &&;
+
+  /**
+   * Decorates this command with a lambda to call on interrupt, following the
+   * command's inherent Command::End(bool) method.
+   *
+   * @param handler a lambda to run when the command is interrupted
+   * @return the decorated command
+   */
+  CommandPtr HandleInterrupt(std::function<void()> handler) &&;
+
+  /**
+   * Decorates this Command with a name. Is an inline function for
+   * Command::SetName(std::string_view);
+   *
+   * @param name name
+   * @return the decorated Command
+   */
+  CommandPtr WithName(std::string_view name) &&;
+
+  /**
+   * Get a raw pointer to the held command.
+   */
+  Command* get() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  Command* get() && = delete;
+
+  /**
+   * Convert to the underlying unique_ptr.
+   */
+  std::unique_ptr<Command> Unwrap() &&;
+
+  /**
+   * Schedules this command.
+   *
+   * @deprecated Use CommandScheduler::GetInstance().Schedule() instead
+   */
+  [[deprecated("Use CommandScheduler::GetInstance().Schedule() instead.")]]
+  void Schedule() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void Schedule() && = delete;
+
+  /**
+   * Cancels this command. Will call End(true). Commands will be canceled
+   * regardless of interruption behavior.
+   */
+  void Cancel() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void Cancel() && = delete;
+
+  /**
+   * Whether or not 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.
+   */
+  bool IsScheduled() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void IsScheduled() && = delete;
+
+  /**
+   * Whether the command requires a given subsystem.  Named "HasRequirement"
+   * rather than "requires" to avoid confusion with Command::Requires(Subsystem)
+   * -- this may be able to be changed in a few years.
+   *
+   * @param requirement the subsystem to inquire about
+   * @return whether the subsystem is required
+   */
+  bool HasRequirement(Subsystem* requirement) const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void HasRequirement(Subsystem* requirement) && = delete;
+
+  /**
+   * Check if this CommandPtr object is valid and wasn't moved-from.
+   */
+  explicit operator bool() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  explicit operator bool() && = delete;
+
+  /**
+   * Convert a vector of CommandPtr objects to their underlying unique_ptrs.
+   */
+  static std::vector<std::unique_ptr<Command>> UnwrapVector(
+      std::vector<CommandPtr>&& vec);
+
+ private:
+  std::unique_ptr<Command> m_ptr;
+  std::string m_moveOutSite{""};
+  void AssertValid() const;
+};
+
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/CommandScheduler.h

@@ -0,0 +1,491 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <functional>
+#include <initializer_list>
+#include <memory>
+#include <optional>
+#include <span>
+#include <utility>
+
+#include <frc/Errors.h>
+#include <frc/Watchdog.h>
+#include <frc/event/EventLoop.h>
+#include <units/time.h>
+#include <wpi/FunctionExtras.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc2 {
+class Command;
+class CommandPtr;
+class Subsystem;
+
+/**
+ * The scheduler responsible for running Commands.  A Command-based robot should
+ * call 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 RegisterSubsystem() in order for their Periodic()
+ * methods to be called and for their default commands to be scheduled.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class CommandScheduler final : public wpi::Sendable,
+                               public wpi::SendableHelper<CommandScheduler> {
+ public:
+  /**
+   * Returns the Scheduler instance.
+   *
+   * @return the instance
+   */
+  static CommandScheduler& GetInstance();
+
+  ~CommandScheduler() override;
+  CommandScheduler(const CommandScheduler&) = delete;
+  CommandScheduler& operator=(const CommandScheduler&) = delete;
+
+  using Action = std::function<void(const Command&)>;
+  using InterruptAction =
+      std::function<void(const Command&, const std::optional<Command*>&)>;
+
+  /**
+   * Changes the period of the loop overrun watchdog. This should be kept in
+   * sync with the TimedRobot period.
+   */
+  void SetPeriod(units::second_t period);
+
+  /**
+   * Get the active button poll.
+   *
+   * @return a reference to the current {@link frc::EventLoop} object polling
+   * buttons.
+   */
+  frc::EventLoop* GetActiveButtonLoop() const;
+
+  /**
+   * Replace the button poll with another one.
+   *
+   * @param loop the new button polling loop object.
+   */
+  void SetActiveButtonLoop(frc::EventLoop* loop);
+
+  /**
+   * Get the default button poll.
+   *
+   * @return a reference to the default {@link frc::EventLoop} object polling
+   * buttons.
+   */
+  frc::EventLoop* GetDefaultButtonLoop() const;
+
+  /**
+   * 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 are
+   * interruptible. If this is the case, they will be interrupted and the
+   * command will be scheduled.
+   *
+   * @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
+   */
+  void Schedule(const CommandPtr& command);
+
+  /**
+   * 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 are
+   * interruptible. If this is the case, they will be interrupted and the
+   * command will be scheduled.
+   *
+   * @param command the command to schedule
+   */
+  void Schedule(CommandPtr&& command);
+
+  /**
+   * 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.
+   *
+   * The pointer must remain valid through the entire lifecycle of the command.
+   *
+   * @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
+   */
+  void Schedule(Command* command);
+
+  /**
+   * Schedules multiple commands for execution. Does nothing for commands
+   * already scheduled.
+   *
+   * @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
+   */
+  void Schedule(std::span<Command* const> commands);
+
+  /**
+   * Schedules multiple commands for execution. Does nothing for commands
+   * already scheduled.
+   *
+   * @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
+   */
+  void Schedule(std::initializer_list<Command*> commands);
+
+  /**
+   * 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.
+   */
+  void Run();
+
+  /**
+   * 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 subsystem the subsystem to register
+   */
+  void RegisterSubsystem(Subsystem* subsystem);
+
+  /**
+   * 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 subsystem the subsystem to un-register
+   */
+  void UnregisterSubsystem(Subsystem* subsystem);
+
+  void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+  void RegisterSubsystem(std::span<Subsystem* const> subsystems);
+
+  void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+  void UnregisterSubsystem(std::span<Subsystem* const> 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.
+   */
+  void UnregisterAllSubsystems();
+
+  /**
+   * 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 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
+   */
+  template <std::derived_from<Command> T>
+  void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
+    if (!defaultCommand.HasRequirement(subsystem)) {
+      throw FRC_MakeError(frc::err::CommandIllegalUse,
+                          "Default commands must require their subsystem!");
+    }
+    SetDefaultCommandImpl(subsystem, std::make_unique<std::decay_t<T>>(
+                                         std::forward<T>(defaultCommand)));
+  }
+
+  /**
+   * 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 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
+   */
+  void SetDefaultCommand(Subsystem* subsystem, CommandPtr&& 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
+   */
+  void RemoveDefaultCommand(Subsystem* subsystem);
+
+  /**
+   * 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
+   */
+  Command* GetDefaultCommand(const Subsystem* subsystem) const;
+
+  /**
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param command the command to cancel
+   */
+  void Cancel(Command* command);
+
+  /**
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param command the command to cancel
+   */
+  void Cancel(const CommandPtr& command);
+
+  /**
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param commands the commands to cancel
+   */
+  void Cancel(std::span<Command* const> commands);
+
+  /**
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param commands the commands to cancel
+   */
+  void Cancel(std::initializer_list<Command*> commands);
+
+  /**
+   * Cancels all commands that are currently scheduled.
+   */
+  void CancelAll();
+
+  /**
+   * 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 of CommandGroups, as the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(std::span<const Command* const> commands) const;
+
+  /**
+   * Whether the given commands are running.  Note that this only works on
+   * commands that are directly scheduled by the scheduler; it will not work on
+   * commands inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(std::initializer_list<const Command*> commands) const;
+
+  /**
+   * Whether a given command is running.  Note that this only works on commands
+   * that are directly scheduled by the scheduler; it will not work on commands
+   * inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param command the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(const Command* command) const;
+
+  /**
+   * Whether a given command is running.  Note that this only works on commands
+   * that are directly scheduled by the scheduler; it will not work on commands
+   * inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param command the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(const CommandPtr& command) const;
+
+  /**
+   * 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
+   */
+  Command* Requiring(const Subsystem* subsystem) const;
+
+  /**
+   * Disables the command scheduler.
+   */
+  void Disable();
+
+  /**
+   * Enables the command scheduler.
+   */
+  void Enable();
+
+  /**
+   * Prints list of epochs added so far and their times.
+   */
+  void PrintWatchdogEpochs();
+
+  /**
+   * Adds an action to perform on the initialization of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInitialize(Action action);
+
+  /**
+   * Adds an action to perform on the execution of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandExecute(Action action);
+
+  /**
+   * Adds an action to perform on the interruption of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInterrupt(Action action);
+
+  /**
+   * 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 nullopt if it was not canceled by a
+   * command (e.g., by Cancel()).
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInterrupt(InterruptAction action);
+
+  /**
+   * Adds an action to perform on the finishing of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandFinish(Action action);
+
+  /**
+   * Requires that the specified command hasn't already been added to a
+   * composition.
+   *
+   * @param command The command to check
+   * @throws if the given commands have already been composed.
+   */
+  void RequireUngrouped(const Command* command);
+
+  /**
+   * Requires that the specified commands have not already been added to a
+   * composition.
+   *
+   * @param commands The commands to check
+   * @throws if the given commands have already been composed.
+   */
+  void RequireUngrouped(std::span<const std::unique_ptr<Command>> commands);
+
+  /**
+   * 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.
+   */
+  void RequireUngrouped(std::initializer_list<const Command*> commands);
+
+  /**
+   * Requires that the specified command has not 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.
+   */
+  void RequireUngroupedAndUnscheduled(const Command* 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.
+   */
+  void RequireUngroupedAndUnscheduled(
+      std::span<const std::unique_ptr<Command>> commands);
+
+  /**
+   * 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.
+   */
+  void RequireUngroupedAndUnscheduled(
+      std::initializer_list<const Command*> commands);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  // Constructor; private as this is a singleton
+  CommandScheduler();
+
+  void SetDefaultCommandImpl(Subsystem* subsystem,
+                             std::unique_ptr<Command> command);
+
+  void Cancel(Command* command, std::optional<Command*> interruptor);
+
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
+
+  frc::Watchdog m_watchdog;
+
+  friend class CommandTestBase;
+
+  template <typename T>
+  friend class CommandTestBaseWithParam;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/Commands.h

@@ -0,0 +1,267 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <functional>
+#include <memory>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <wpi/deprecated.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Requirements.h"
+#include "frc2/command/SelectCommand.h"
+
+namespace frc2 {
+class Subsystem;
+
+/**
+ * Namespace for command factories.
+ */
+namespace cmd {
+
+/**
+ * Constructs a command that does nothing, finishing immediately.
+ */
+CommandPtr None();
+
+/**
+ * Constructs a command that does nothing until interrupted.
+ *
+ * @param requirements Subsystems to require
+ * @return the command
+ */
+CommandPtr Idle(Requirements 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
+ */
+CommandPtr RunOnce(std::function<void()> action,
+                   Requirements requirements = {});
+
+/**
+ * Constructs a command that runs an action every iteration until interrupted.
+ *
+ * @param action the action to run
+ * @param requirements subsystems the action requires
+ */
+CommandPtr Run(std::function<void()> action, Requirements 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
+ */
+CommandPtr StartEnd(std::function<void()> start, std::function<void()> end,
+                    Requirements 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
+ */
+CommandPtr RunEnd(std::function<void()> run, std::function<void()> end,
+                  Requirements 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
+ */
+CommandPtr StartRun(std::function<void()> start, std::function<void()> run,
+                    Requirements requirements = {});
+
+/**
+ * Constructs a command that prints a message and finishes.
+ *
+ * @param msg the message to print
+ */
+CommandPtr Print(std::string_view msg);
+
+// Idling Commands
+
+/**
+ * Constructs a command that does nothing, finishing after a specified duration.
+ *
+ * @param duration after how long the command finishes
+ */
+CommandPtr Wait(units::second_t duration);
+
+/**
+ * Constructs a command that does nothing, finishing once a condition becomes
+ * true.
+ *
+ * @param condition the condition
+ */
+CommandPtr WaitUntil(std::function<bool()> 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
+ */
+CommandPtr Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
+                  std::function<bool()> selector);
+
+/**
+ * Runs one of several commands, based on the selector function.
+ *
+ * @param selector the selector function
+ * @param commands map of commands to select from
+ */
+template <typename Key, std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr Select(std::function<Key()> selector,
+                  std::pair<Key, CommandPtrs>&&... commands) {
+  std::vector<std::pair<Key, std::unique_ptr<Command>>> vec;
+
+  ((void)vec.emplace_back(commands.first, std::move(commands.second).Unwrap()),
+   ...);
+
+  return SelectCommand(std::move(selector), std::move(vec)).ToPtr();
+}
+
+/**
+ * Runs the command supplied by the supplier.
+ *
+ * @param supplier the command supplier
+ * @param requirements the set of requirements for this command
+ */
+CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier,
+                 Requirements 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
+ */
+CommandPtr DeferredProxy(wpi::unique_function<Command*()> supplier);
+
+/**
+ * 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
+ */
+CommandPtr DeferredProxy(wpi::unique_function<CommandPtr()> supplier);
+// Command Groups
+
+namespace impl {
+
+/**
+ * Create a vector of commands.
+ */
+template <std::convertible_to<CommandPtr>... Args>
+std::vector<CommandPtr> MakeVector(Args&&... args) {
+  std::vector<CommandPtr> data;
+  data.reserve(sizeof...(Args));
+  (data.emplace_back(std::forward<Args>(args)), ...);
+  return data;
+}
+
+}  // namespace impl
+
+/**
+ * Runs a group of commands in series, one after the other.
+ */
+CommandPtr Sequence(std::vector<CommandPtr>&& commands);
+
+/**
+ * Runs a group of commands in series, one after the other.
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr Sequence(CommandPtrs&&... commands) {
+  return Sequence(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
+}
+
+/**
+ * Runs a group of commands in series, one after the other. Once the last
+ * command ends, the group is restarted.
+ */
+CommandPtr RepeatingSequence(std::vector<CommandPtr>&& commands);
+
+/**
+ * Runs a group of commands in series, one after the other. Once the last
+ * command ends, the group is restarted.
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr RepeatingSequence(CommandPtrs&&... commands) {
+  return RepeatingSequence(
+      impl::MakeVector(std::forward<CommandPtrs>(commands)...));
+}
+
+/**
+ * Runs a group of commands at the same time. Ends once all commands in the
+ * group finish.
+ */
+CommandPtr Parallel(std::vector<CommandPtr>&& commands);
+
+/**
+ * Runs a group of commands at the same time. Ends once all commands in the
+ * group finish.
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr Parallel(CommandPtrs&&... commands) {
+  return Parallel(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
+}
+
+/**
+ * Runs a group of commands at the same time. Ends once any command in the group
+ * finishes, and cancels the others.
+ */
+CommandPtr Race(std::vector<CommandPtr>&& commands);
+
+/**
+ * Runs a group of commands at the same time. Ends once any command in the group
+ * finishes, and cancels the others.
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr Race(CommandPtrs&&... commands) {
+  return Race(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
+}
+
+/**
+ * Runs a group of commands at the same time. Ends once a specific command
+ * finishes, and cancels the others.
+ */
+CommandPtr Deadline(CommandPtr&& deadline, std::vector<CommandPtr>&& others);
+
+/**
+ * Runs a group of commands at the same time. Ends once a specific command
+ * finishes, and cancels the others.
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+CommandPtr Deadline(CommandPtr&& deadline, CommandPtrs&&... commands) {
+  return Deadline(std::move(deadline),
+                  impl::MakeVector(std::forward<CommandPtrs>(commands)...));
+}
+
+}  // namespace cmd
+
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/ConditionalCommand.h

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ *
+ * @see ScheduleCommand
+ */
+class ConditionalCommand : public CommandHelper<Command, ConditionalCommand> {
+ public:
+  /**
+   * 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
+   */
+  template <std::derived_from<Command> Command1,
+            std::derived_from<Command> Command2>
+  ConditionalCommand(Command1&& onTrue, Command2&& onFalse,
+                     std::function<bool()> condition)
+      : ConditionalCommand(std::make_unique<std::decay_t<Command1>>(
+                               std::forward<Command1>(onTrue)),
+                           std::make_unique<std::decay_t<Command2>>(
+                               std::forward<Command2>(onFalse)),
+                           condition) {}
+
+  /**
+   * 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
+   */
+  ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                     std::unique_ptr<Command>&& onFalse,
+                     std::function<bool()> condition);
+
+  ConditionalCommand(ConditionalCommand&& other) = default;
+
+  // No copy constructors for command groups
+  ConditionalCommand(const ConditionalCommand& other) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+  InterruptionBehavior GetInterruptionBehavior() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::unique_ptr<Command> m_onTrue;
+  std::unique_ptr<Command> m_onFalse;
+  std::function<bool()> m_condition;
+  Command* m_selectedCommand{nullptr};
+  bool m_runsWhenDisabled = true;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/DeferredCommand.h

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/FunctionExtras.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * 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.
+ *
+ * Note that the supplier <i>must</i> create a new Command each call. For
+ * selecting one of a preallocated set of commands, use SelectCommand.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+class DeferredCommand : public CommandHelper<Command, DeferredCommand> {
+ public:
+  /**
+   * 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 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.
+   *
+   */
+  DeferredCommand(wpi::unique_function<CommandPtr()> supplier,
+                  Requirements requirements);
+
+  DeferredCommand(DeferredCommand&& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  wpi::unique_function<CommandPtr()> m_supplier;
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/FunctionalCommand.h

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * 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.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class FunctionalCommand : public CommandHelper<Command, FunctionalCommand> {
+ public:
+  /**
+   * 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
+   */
+  FunctionalCommand(std::function<void()> onInit,
+                    std::function<void()> onExecute,
+                    std::function<void(bool)> onEnd,
+                    std::function<bool()> isFinished,
+                    Requirements requirements = {});
+
+  FunctionalCommand(FunctionalCommand&& other) = default;
+
+  FunctionalCommand(const FunctionalCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  std::function<void()> m_onInit;
+  std::function<void()> m_onExecute;
+  std::function<void(bool)> m_onEnd;
+  std::function<bool()> m_isFinished;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/InstantCommand.h

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * 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.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class InstantCommand : public CommandHelper<FunctionalCommand, InstantCommand> {
+ public:
+  /**
+   * 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
+   */
+  explicit InstantCommand(std::function<void()> toRun,
+                          Requirements requirements = {});
+
+  InstantCommand(InstantCommand&& other) = default;
+
+  InstantCommand(const InstantCommand& other) = default;
+
+  /**
+   * Creates a new InstantCommand with a Runnable that does nothing.  Useful
+   * only as a no-arg constructor to call implicitly from subclass constructors.
+   */
+  InstantCommand();
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/NotifierCommand.h

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include <frc/Notifier.h>
+#include <units/time.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * 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
+ * Command::WithTimeout(double) or Command::Until(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.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class NotifierCommand : public CommandHelper<Command, NotifierCommand> {
+ public:
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun        the runnable for the notifier to run
+   * @param period       the period at which the notifier should run
+   * @param requirements the subsystems required by this command
+   */
+  NotifierCommand(std::function<void()> toRun, units::second_t period,
+                  Requirements requirements = {});
+
+  NotifierCommand(NotifierCommand&& other);
+
+  NotifierCommand(const NotifierCommand& other);
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+ private:
+  std::function<void()> m_toRun;
+  frc::Notifier m_notifier;
+  units::second_t m_period;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/ParallelCommandGroup.h

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <wpi/DecayedDerivedFrom.h>
+
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class ParallelCommandGroup
+    : public CommandHelper<Command, ParallelCommandGroup> {
+ public:
+  /**
+   * Creates a new ParallelCommandGroup. The given commands will be executed
+   * simultaneously. The command group 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.
+   */
+  explicit ParallelCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * Creates a new ParallelCommandGroup. The given commands will be executed
+   * simultaneously. The command group 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.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  explicit ParallelCommandGroup(Commands&&... commands) {
+    AddCommands(std::forward<Commands>(commands)...);
+  }
+
+  ParallelCommandGroup(ParallelCommandGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelCommandGroup(const ParallelCommandGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelCommandGroup(ParallelCommandGroup&) = delete;
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  void AddCommands(Commands&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::decay_t<Commands>>(
+         std::forward<Commands>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() final;
+
+  void Execute() final;
+
+  void End(bool interrupted) final;
+
+  bool IsFinished() final;
+
+  bool RunsWhenDisabled() const override;
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands);
+
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+  bool m_runWhenDisabled{true};
+  Command::InterruptionBehavior m_interruptBehavior{
+      Command::InterruptionBehavior::kCancelIncoming};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/ParallelDeadlineGroup.h

@@ -0,0 +1,118 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <wpi/DecayedDerivedFrom.h>
+
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class ParallelDeadlineGroup
+    : public CommandHelper<Command, ParallelDeadlineGroup> {
+ public:
+  /**
+   * 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 commands the commands to be executed
+   */
+  ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
+                        std::vector<std::unique_ptr<Command>>&& commands);
+  /**
+   * 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 commands the commands to be executed
+   */
+  template <wpi::DecayedDerivedFrom<Command> T,
+            wpi::DecayedDerivedFrom<Command>... Commands>
+  explicit ParallelDeadlineGroup(T&& deadline, Commands&&... commands) {
+    SetDeadline(std::make_unique<std::decay_t<T>>(std::forward<T>(deadline)));
+    AddCommands(std::forward<Commands>(commands)...);
+  }
+
+  ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  void AddCommands(Commands&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::decay_t<Commands>>(
+         std::forward<Commands>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() final;
+
+  void Execute() final;
+
+  void End(bool interrupted) final;
+
+  bool IsFinished() final;
+
+  bool RunsWhenDisabled() const override;
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands);
+
+  void SetDeadline(std::unique_ptr<Command>&& deadline);
+
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+  Command* m_deadline;
+  bool m_runWhenDisabled{true};
+  Command::InterruptionBehavior m_interruptBehavior{
+      Command::InterruptionBehavior::kCancelIncoming};
+  bool m_finished{true};
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/ParallelRaceGroup.h

@@ -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.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <wpi/DecayedDerivedFrom.h>
+
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class ParallelRaceGroup : public CommandHelper<Command, ParallelRaceGroup> {
+ public:
+  /**
+   * 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.
+   */
+  explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
+
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  explicit ParallelRaceGroup(Commands&&... commands) {
+    AddCommands(std::forward<Commands>(commands)...);
+  }
+
+  ParallelRaceGroup(ParallelRaceGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelRaceGroup(const ParallelRaceGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  ParallelRaceGroup(ParallelRaceGroup&) = delete;
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add to the group.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  void AddCommands(Commands&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::decay_t<Commands>>(
+         std::forward<Commands>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() final;
+
+  void Execute() final;
+
+  void End(bool interrupted) final;
+
+  bool IsFinished() final;
+
+  bool RunsWhenDisabled() const override;
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands);
+
+  std::vector<std::unique_ptr<Command>> m_commands;
+  bool m_runWhenDisabled{true};
+  Command::InterruptionBehavior m_interruptBehavior{
+      Command::InterruptionBehavior::kCancelIncoming};
+  bool m_finished{false};
+  bool isRunning = false;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/PrintCommand.h

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/InstantCommand.h"
+
+namespace frc2 {
+/**
+ * A command that prints a string when initialized.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
+ public:
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  explicit PrintCommand(std::string_view message);
+
+  PrintCommand(PrintCommand&& other) = default;
+
+  PrintCommand(const PrintCommand& other) = default;
+
+  bool RunsWhenDisabled() const override;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/ProxyCommand.h

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/FunctionExtras.h>
+#include <wpi/deprecated.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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
+ */
+class ProxyCommand : public CommandHelper<Command, ProxyCommand> {
+ public:
+  /**
+   * 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
+   */
+  WPI_IGNORE_DEPRECATED
+  [[deprecated("Defer a proxy command instead.")]]
+  explicit ProxyCommand(wpi::unique_function<Command*()> supplier);
+
+  /**
+   * 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("Defer a proxy command instead.")]]
+  explicit ProxyCommand(wpi::unique_function<CommandPtr()> supplier);
+  WPI_UNIGNORE_DEPRECATED
+
+  /**
+   * 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
+   */
+  explicit ProxyCommand(Command* command);
+
+  /**
+   * Creates a new ProxyCommand that schedules the given command when
+   * initialized, and ends when it is no longer scheduled.
+   *
+   * <p>Note that this constructor passes ownership of the given command to the
+   * returned ProxyCommand.
+   *
+   * @param command the command to schedule
+   */
+  explicit ProxyCommand(std::unique_ptr<Command> command);
+
+  ProxyCommand(ProxyCommand&& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  wpi::unique_function<Command*()> m_supplier;
+  Command* m_command = nullptr;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/RepeatCommand.h

@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <concepts>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and 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
+ */
+class RepeatCommand : public CommandHelper<Command, RepeatCommand> {
+ public:
+  /**
+   * 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
+   */
+  explicit RepeatCommand(std::unique_ptr<Command>&& command);
+
+  /**
+   * 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
+   */
+  template <std::derived_from<Command> T>
+  // NOLINTNEXTLINE(bugprone-forwarding-reference-overload)
+  explicit RepeatCommand(T&& command)
+      : RepeatCommand(
+            std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
+
+  RepeatCommand(RepeatCommand&& other) = default;
+
+  // No copy constructors for command groups
+  RepeatCommand(const RepeatCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  RepeatCommand(RepeatCommand&) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  bool IsFinished() override;
+
+  void End(bool interrupted) override;
+
+  bool RunsWhenDisabled() const override;
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::unique_ptr<Command> m_command;
+  bool m_ended;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/Requirements.h

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <initializer_list>
+#include <span>
+#include <vector>
+
+#include "frc2/command/Subsystem.h"
+
+namespace frc2 {
+
+/**
+ * Represents requirements for a command, which is a set of (pointers to)
+ * subsystems. This class is implicitly convertible from std::initializer_list
+ * and std::span.
+ */
+class Requirements {
+ public:
+  // NOLINTNEXTLINE
+  /*implicit*/ Requirements(std::initializer_list<Subsystem*> requirements)
+      : m_subsystems{requirements.begin(), requirements.end()} {}
+
+  // NOLINTNEXTLINE
+  /*implicit*/ Requirements(std::span<Subsystem* const> requirements)
+      : m_subsystems{requirements.begin(), requirements.end()} {}
+
+  Requirements() = default;
+
+  Requirements(const Requirements&) = default;
+
+  std::vector<Subsystem*>::const_iterator begin() const {
+    return m_subsystems.begin();
+  }
+
+  std::vector<Subsystem*>::const_iterator end() const {
+    return m_subsystems.end();
+  }
+
+ private:
+  std::vector<Subsystem*> m_subsystems;
+};
+
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/RunCommand.h

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * A command that runs a Runnable continuously.  Has no end condition as-is;
+ * either subclass it or use Command.WithTimeout() or
+ * Command.Until() to give it one.  If you only wish
+ * to execute a Runnable once, use InstantCommand.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class RunCommand : public CommandHelper<FunctionalCommand, RunCommand> {
+ public:
+  /**
+   * 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
+   */
+  explicit RunCommand(std::function<void()> toRun,
+                      Requirements requirements = {});
+
+  RunCommand(RunCommand&& other) = default;
+
+  RunCommand(const RunCommand& other) = default;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/ScheduleCommand.h

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <span>
+
+#include <wpi/SmallSet.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * 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.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class ScheduleCommand : public CommandHelper<Command, ScheduleCommand> {
+ public:
+  /**
+   * Creates a new ScheduleCommand that schedules the given commands when
+   * initialized.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  explicit ScheduleCommand(std::span<Command* const> toSchedule);
+
+  explicit ScheduleCommand(Command* toSchedule);
+
+  ScheduleCommand(ScheduleCommand&& other) = default;
+
+  ScheduleCommand(const ScheduleCommand& other) = default;
+
+  void Initialize() override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  wpi::SmallSet<Command*, 4> m_toSchedule;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/SelectCommand.h

@@ -0,0 +1,163 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <concepts>
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/PrintCommand.h"
+
+namespace frc2 {
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+template <typename Key>
+class SelectCommand : public CommandHelper<Command, SelectCommand<Key>> {
+ public:
+  /**
+   * Creates a new SelectCommand.
+   *
+   * @param commands the map of commands to choose from
+   * @param selector the selector to determine which command to run
+   */
+  template <std::derived_from<Command>... Commands>
+  explicit SelectCommand(std::function<Key()> selector,
+                         std::pair<Key, Commands>... commands)
+      : m_selector{std::move(selector)} {
+    std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
+
+    ((void)foo.emplace_back(
+         commands.first,
+         std::make_unique<std::decay_t<Commands>>(std::move(commands.second))),
+     ...);
+
+    m_defaultCommand.SetComposed(true);
+    for (auto&& command : foo) {
+      CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+          command.second.get());
+      command.second.get()->SetComposed(true);
+    }
+
+    for (auto&& command : foo) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      if (command.second->GetInterruptionBehavior() ==
+          Command::InterruptionBehavior::kCancelSelf) {
+        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+      }
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  SelectCommand(
+      std::function<Key()> selector,
+      std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
+      : m_selector{std::move(selector)} {
+    m_defaultCommand.SetComposed(true);
+    for (auto&& command : commands) {
+      CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+          command.second.get());
+      command.second.get()->SetComposed(true);
+    }
+
+    for (auto&& command : commands) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      if (command.second->GetInterruptionBehavior() ==
+          Command::InterruptionBehavior::kCancelSelf) {
+        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
+      }
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  // No copy constructors for command groups
+  SelectCommand(const SelectCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  SelectCommand(SelectCommand&) = delete;
+
+  SelectCommand(SelectCommand&& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override { m_selectedCommand->Execute(); }
+
+  void End(bool interrupted) override {
+    return m_selectedCommand->End(interrupted);
+  }
+
+  bool IsFinished() override { return m_selectedCommand->IsFinished(); }
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override {
+    return m_interruptBehavior;
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override {
+    Command::InitSendable(builder);
+
+    builder.AddStringProperty(
+        "selected",
+        [this] {
+          if (m_selectedCommand) {
+            return m_selectedCommand->GetName();
+          } else {
+            return std::string{"null"};
+          }
+        },
+        nullptr);
+  }
+
+ private:
+  std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
+  std::function<Key()> m_selector;
+  Command* m_selectedCommand;
+  bool m_runsWhenDisabled = true;
+  Command::InterruptionBehavior m_interruptBehavior{
+      Command::InterruptionBehavior::kCancelIncoming};
+
+  PrintCommand m_defaultCommand{
+      "SelectCommand selector value does not correspond to any command!"};
+};
+
+template <typename T>
+void SelectCommand<T>::Initialize() {
+  auto find = m_commands.find(m_selector());
+  if (find == m_commands.end()) {
+    m_selectedCommand = &m_defaultCommand;
+  } else {
+    m_selectedCommand = find->second.get();
+  }
+  m_selectedCommand->Initialize();
+}
+
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/SequentialCommandGroup.h

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <limits>
+#include <memory>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <wpi/DecayedDerivedFrom.h>
+
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+
+const size_t invalid_index = std::numeric_limits<size_t>::max();
+
+/**
+ * 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 are owned by the composition and cannot be added to any other
+ * composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class SequentialCommandGroup
+    : public CommandHelper<Command, SequentialCommandGroup> {
+ public:
+  /**
+   * 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.
+   */
+  explicit SequentialCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * 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.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  explicit SequentialCommandGroup(Commands&&... commands) {
+    AddCommands(std::forward<Commands>(commands)...);
+  }
+
+  SequentialCommandGroup(SequentialCommandGroup&& other) = default;
+
+  // No copy constructors for command groups
+  SequentialCommandGroup(const SequentialCommandGroup&) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  SequentialCommandGroup(SequentialCommandGroup&) = delete;
+
+  /**
+   * Adds the given commands to the group.
+   *
+   * @param commands Commands to add, in order of execution.
+   */
+  template <wpi::DecayedDerivedFrom<Command>... Commands>
+  void AddCommands(Commands&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::decay_t<Commands>>(
+         std::forward<Commands>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() final;
+
+  void Execute() final;
+
+  void End(bool interrupted) final;
+
+  bool IsFinished() final;
+
+  bool RunsWhenDisabled() const override;
+
+  Command::InterruptionBehavior GetInterruptionBehavior() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands);
+
+  wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
+  size_t m_currentCommandIndex{invalid_index};
+  bool m_runWhenDisabled{true};
+  Command::InterruptionBehavior m_interruptBehavior{
+      Command::InterruptionBehavior::kCancelIncoming};
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/StartEndCommand.h

@@ -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.
+
+#pragma once
+
+#include <functional>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * 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 Command.WithTimeout() or Command.Until() to give
+ * it one.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class StartEndCommand
+    : public CommandHelper<FunctionalCommand, StartEndCommand> {
+ public:
+  /**
+   * 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
+   */
+  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+                  Requirements requirements = {});
+
+  StartEndCommand(StartEndCommand&& other) = default;
+
+  StartEndCommand(const StartEndCommand& other) = default;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/Subsystem.h

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+#include <functional>
+#include <string>
+#include <utility>
+
+#include <wpi/FunctionExtras.h>
+
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+class Command;
+class CommandPtr;
+/**
+ * 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 Commands.  Subsystems are used by the 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 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
+ * CommandScheduler.RegisterSubsystem() method in order for the
+ * Periodic() method to be called.  It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations.  The
+ * SubsystemBase class offers a simple base for user implementations
+ * that handles this.
+ *
+ * This class is provided by the NewCommands VendorDep
+ *
+ * @see Command
+ * @see CommandScheduler
+ * @see SubsystemBase
+ */
+class Subsystem {
+ public:
+  virtual ~Subsystem();
+  /**
+   * This method is called periodically by the CommandScheduler.  Useful for
+   * updating subsystem-specific state that you don't want to offload to a
+   * 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.
+   */
+  virtual void Periodic();
+
+  /**
+   * This method is called periodically by the CommandScheduler.  Useful for
+   * updating subsystem-specific state that needs to be maintained for
+   * simulations, such as for updating simulation classes and setting simulated
+   * sensor readings.
+   */
+  virtual void SimulationPeriodic();
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  virtual std::string GetName() const;
+
+  /**
+   * Sets the default 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 IsFinished() method should always return false.  Will automatically
+   * register this subsystem with the CommandScheduler.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  template <std::derived_from<Command> T>
+  void SetDefaultCommand(T&& defaultCommand) {
+    CommandScheduler::GetInstance().SetDefaultCommand(
+        this, std::forward<T>(defaultCommand));
+  }
+
+  /**
+   * Sets the default 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 IsFinished() method should always return false.  Will automatically
+   * register this subsystem with the CommandScheduler.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  void SetDefaultCommand(CommandPtr&& defaultCommand);
+
+  /**
+   * Removes the default command for the subsystem.  This will not cancel the
+   * default command if it is currently running.
+   */
+  void RemoveDefaultCommand();
+
+  /**
+   * 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
+   */
+  Command* GetDefaultCommand() const;
+
+  /**
+   * 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
+   */
+  Command* GetCurrentCommand() const;
+
+  /**
+   * Registers this subsystem with the CommandScheduler, allowing its
+   * Periodic() method to be called when the scheduler runs.
+   */
+  void Register();
+
+  /**
+   * Constructs a command that does nothing until interrupted. Requires this
+   * subsystem.
+   *
+   * @return the command
+   */
+  [[nodiscard]]
+  CommandPtr Idle();
+
+  /**
+   * Constructs a command that runs an action once and finishes. Requires this
+   * subsystem.
+   *
+   * @param action the action to run
+   */
+  CommandPtr RunOnce(std::function<void()> action);
+
+  /**
+   * Constructs a command that runs an action every iteration until interrupted.
+   * Requires this subsystem.
+   *
+   * @param action the action to run
+   */
+  CommandPtr Run(std::function<void()> action);
+
+  /**
+   * 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
+   */
+  CommandPtr StartEnd(std::function<void()> start, std::function<void()> end);
+
+  /**
+   * 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
+   */
+  CommandPtr RunEnd(std::function<void()> run, std::function<void()> end);
+
+  /**
+   * Constructs a command that runs an action once, and then runs an action
+   * every iteration until interrupted. Requires this subsystem.
+   *
+   * @param start the action to run on start
+   * @param run the action to run every iteration
+   */
+  CommandPtr StartRun(std::function<void()> start, std::function<void()> run);
+
+  /**
+   * Constructs a DeferredCommand with the provided supplier. This subsystem is
+   * added as a requirement.
+   *
+   * @param supplier the command supplier.
+   * @return the command.
+   */
+  CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier);
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/SubsystemBase.h

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc2/command/Subsystem.h"
+
+namespace frc2 {
+/**
+ * A base for subsystems that handles registration in the constructor, and
+ * provides a more intuitive method for setting the default command.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class SubsystemBase : public Subsystem,
+                      public wpi::Sendable,
+                      public wpi::SendableHelper<SubsystemBase> {
+ public:
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  std::string GetName() const override;
+
+  /**
+   * Sets the name of this Subsystem.
+   *
+   * @param name name
+   */
+  void SetName(std::string_view name);
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Subsystem.
+   *
+   * @param name subsystem name
+   */
+  void SetSubsystem(std::string_view name);
+
+  /**
+   * Associate a Sendable with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  void AddChild(std::string name, wpi::Sendable* child);
+
+ protected:
+  /**
+   * Constructor.  Telemetry/log name defaults to the classname.
+   */
+  SubsystemBase();
+  /**
+   * Constructor.
+   *
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  explicit SubsystemBase(std::string_view name);
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/WaitCommand.h

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Timer.h>
+#include <units/time.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but takes a specified amount of time to finish.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class WaitCommand : public CommandHelper<Command, WaitCommand> {
+ public:
+  /**
+   * Creates a new WaitCommand.  This command will do nothing, and end after the
+   * specified duration.
+   *
+   * @param duration the time to wait
+   */
+  explicit WaitCommand(units::second_t duration);
+
+  WaitCommand(WaitCommand&& other) = default;
+
+  WaitCommand(const WaitCommand& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ protected:
+  /// The timer used for waiting.
+  frc::Timer m_timer;
+
+ private:
+  units::second_t m_duration;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/WaitUntilCommand.h

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include <units/time.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but ends after a specified match time or
+ * condition.  Useful for CommandGroups.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class WaitUntilCommand : public CommandHelper<Command, WaitUntilCommand> {
+ public:
+  /**
+   * Creates a new WaitUntilCommand that ends after a given condition becomes
+   * true.
+   *
+   * @param condition the condition to determine when to end
+   */
+  explicit WaitUntilCommand(std::function<bool()> condition);
+
+  /**
+   * 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
+   */
+  explicit WaitUntilCommand(units::second_t time);
+
+  WaitUntilCommand(WaitUntilCommand&& other) = default;
+
+  WaitUntilCommand(const WaitUntilCommand& other) = default;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  std::function<bool()> m_condition;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/WrapperCommand.h

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <concepts>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A class used internally to wrap commands while overriding a specific method;
+ * all other methods will call through to the wrapped command.
+ *
+ * <p>Wrapped commands may only be used through the wrapper, trying to directly
+ * schedule them or add them to a group will throw an exception.
+ */
+class WrapperCommand : public CommandHelper<Command, WrapperCommand> {
+ public:
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this
+   * command or add it to a group will throw an exception.
+   */
+  explicit WrapperCommand(std::unique_ptr<Command>&& command);
+
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this
+   * command or add it to a group will throw an exception.
+   */
+  template <std::derived_from<Command> T>
+  // NOLINTNEXTLINE(bugprone-forwarding-reference-overload)
+  explicit WrapperCommand(T&& command)
+      : WrapperCommand(
+            std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
+
+  WrapperCommand(WrapperCommand&& other) = default;
+
+  // No copy constructors for command groups
+  WrapperCommand(const WrapperCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  WrapperCommand(WrapperCommand&) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  bool IsFinished() override;
+
+  void End(bool interrupted) override;
+
+  bool RunsWhenDisabled() const override;
+
+  InterruptionBehavior GetInterruptionBehavior() const override;
+
+  wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
+
+ protected:
+  /// Command being wrapped.
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

commandsv2/src/main/native/include/frc2/command/button/CommandGamepad.h

@@ -0,0 +1,434 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include <frc/Gamepad.h>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/button/CommandGenericHID.h"
+#include "frc2/command/button/Trigger.h"
+
+namespace frc2 {
+/**
+ * A version of {@link frc::Gamepad} with {@link Trigger} factories for
+ * command-based.
+ *
+ * @see frc::Gamepad
+ */
+class CommandGamepad : public CommandGenericHID {
+ public:
+  /**
+   * Construct an instance of a controller.
+   *
+   * @param port The port index on the Driver Station that the controller is
+   * plugged into.
+   */
+  explicit CommandGamepad(int port);
+
+  /**
+   * Get the underlying GenericHID object.
+   *
+   * @return the wrapped GenericHID object
+   */
+  frc::Gamepad& GetHID();
+
+  /**
+   * Constructs a Trigger instance around the South Face button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the South Face button's
+   * digital signal attached to the given loop.
+   */
+  Trigger SouthFace(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the East Face button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the East Face button's
+   * digital signal attached to the given loop.
+   */
+  Trigger EastFace(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the West Face button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the West Face button's
+   * digital signal attached to the given loop.
+   */
+  Trigger WestFace(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the North Face button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the North Face button's
+   * digital signal attached to the given loop.
+   */
+  Trigger NorthFace(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Back button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Back button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Back(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                          .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Guide button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Guide button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Guide(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Start button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Start button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Start(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the left stick button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the left stick button's
+   * digital signal attached to the given loop.
+   */
+  Trigger LeftStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the right stick button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the right stick button's
+   * digital signal attached to the given loop.
+   */
+  Trigger RightStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the right shoulder button's
+   * digital signal attached to the given loop.
+   */
+  Trigger LeftShoulder(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the right shoulder button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the right shoulder button's
+   * digital signal attached to the given loop.
+   */
+  Trigger RightShoulder(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the D-pad up button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the D-pad up button's
+   * digital signal attached to the given loop.
+   */
+  Trigger DpadUp(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                            .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the D-pad down button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the D-pad down button's
+   * digital signal attached to the given loop.
+   */
+  Trigger DpadDown(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the D-pad left button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the D-pad left button's
+   * digital signal attached to the given loop.
+   */
+  Trigger DpadLeft(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the D-pad right button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the D-pad right button's
+   * digital signal attached to the given loop.
+   */
+  Trigger DpadRight(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 1 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 1 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc1(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 1 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Right Paddle 1 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger RightPaddle1(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 1 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Left Paddle 1 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger LeftPaddle1(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Right Paddle 2 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Right Paddle 2 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger RightPaddle2(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Left Paddle 2 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Left Paddle 2 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger LeftPaddle2(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Touchpad button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Touchpad button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Touchpad(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 2 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 2 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc2(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 3 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 3 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc3(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 4 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 4 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc4(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 5 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 5 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc5(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance around the Miscellaneous 6 button's
+   * digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return a Trigger instance representing the Miscellaneous 6 button's
+   * digital signal attached to the given loop.
+   */
+  Trigger Misc6(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * 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 Trigger to be
+   * true. This value should be in the range [0, 1] where 0 is the unpressed
+   * state of the axis. Defaults to 0.5.
+   * @param loop the event loop instance to attach the Trigger to. Defaults to
+   * the CommandScheduler's default loop.
+   * @return a Trigger instance that is true when the left trigger's axis
+   * exceeds the provided threshold, attached to the given loop
+   */
+  Trigger LeftTrigger(double threshold = 0.5,
+                      frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * 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 Trigger to be
+   * true. This value should be in the range [0, 1] where 0 is the unpressed
+   * state of the axis. Defaults to 0.5.
+   * @param loop the event loop instance to attach the Trigger to. Defaults to
+   * the CommandScheduler's default loop.
+   * @return a Trigger instance that is true when the right trigger's axis
+   * exceeds the provided threshold, attached to the given loop
+   */
+  Trigger RightTrigger(
+      double threshold = 0.5,
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Get the X axis value of left side of the controller. Right is positive.
+   *
+   * @return The axis value.
+   */
+  double GetLeftX() const;
+
+  /**
+   * Get the Y axis value of left side of the controller. Back is positive.
+   *
+   * @return The axis value.
+   */
+  double GetLeftY() const;
+
+  /**
+   * Get the X axis value of right side of the controller. Right is positive.
+   *
+   * @return The axis value.
+   */
+  double GetRightX() const;
+
+  /**
+   * Get the Y axis value of right side of the controller. Back is positive.
+   *
+   * @return The axis value.
+   */
+  double GetRightY() const;
+
+  /**
+   * 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.
+   */
+  double GetLeftTriggerAxis() const;
+
+  /**
+   * 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.
+   */
+  double GetRightTriggerAxis() const;
+
+ private:
+  frc::Gamepad m_hid;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/CommandGenericHID.h

@@ -0,0 +1,264 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/DriverStation.h>
+#include <frc/GenericHID.h>
+
+#include "Trigger.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+
+/**
+ * A version of {@link frc::GenericHID} with {@link Trigger} factories for
+ * command-based.
+ *
+ * @see GenericHID
+ */
+class CommandGenericHID {
+ public:
+  /**
+   * Construct an instance of a device.
+   *
+   * @param port The port index on the Driver Station that the device is plugged
+   * into.
+   */
+  explicit CommandGenericHID(int port);
+
+  /**
+   * Get the underlying GenericHID object.
+   *
+   * @return the wrapped GenericHID object
+   */
+  frc::GenericHID& GetHID();
+
+  /**
+   * 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. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the button's digital signal attached
+   * to the given loop.
+   */
+  Trigger Button(int button,
+                 frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                            .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around this angle of a POV on the HID.
+   *
+   * @param loop  the event loop instance to attach the event to. Defaults 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.
+   */
+  Trigger POV(frc::DriverStation::POVDirection angle,
+              frc::EventLoop* loop =
+                  CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around this angle of a POV on the HID.
+   *
+   * @param loop  the event loop instance to attach the event to. Defaults to
+   * {@link CommandScheduler::GetDefaultButtonLoop() the default command
+   * scheduler button loop}.
+   * @param pov   index of the POV to read (starting at 0). Defaults to 0.
+   * @param angle POV angle.
+   * @return a Trigger instance based around this angle of a POV on the HID.
+   */
+  Trigger POV(int pov, frc::DriverStation::POVDirection angle,
+              frc::EventLoop* loop =
+                  CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the up direction of the
+   * default (index 0) POV on the HID.
+   *
+   * @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 the up direction of a POV on the
+   * HID.
+   */
+  Trigger POVUp(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                           .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the up right direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @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 the up right direction of a POV on
+   * the HID.
+   */
+  Trigger POVUpRight(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the right direction of
+   * the default (index 0) POV on the HID.
+   *
+   * @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 the right direction of a POV on the
+   * HID.
+   */
+  Trigger POVRight(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                              .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the down right direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @return a Trigger instance based around the down right direction of a POV
+   * on the HID.
+   */
+  Trigger POVDownRight(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the down direction of
+   * the default (index 0) POV on the HID.
+   *
+   * @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 the down direction of a POV on
+   * the HID.
+   */
+  Trigger POVDown(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                             .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the down left direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @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 the down left direction of a POV on
+   * the HID.
+   */
+  Trigger POVDownLeft(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                                 .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the left direction of
+   * the default (index 0) POV on the HID.
+   *
+   * @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 the left direction of a POV on
+   * the HID.
+   */
+  Trigger POVLeft(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                             .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the up left direction
+   * of the default (index 0) POV on the HID.
+   *
+   * @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 the up left direction of a POV on
+   * the HID.
+   */
+  Trigger POVUpLeft(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance based around the center (not pressed)
+   * position of the default (index 0) POV on the HID.
+   *
+   * @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 the center position of a POV on the
+   * HID.
+   */
+  Trigger POVCenter(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+
+  /**
+   * 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.
+   * @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 that is true when the axis value is less than
+   * the provided threshold.
+   */
+  Trigger AxisLessThan(
+      int axis, double threshold,
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs a Trigger instance that is true when the axis 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 below which this trigger should return true.
+   * @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 that is true when the axis value is greater than
+   * the provided threshold.
+   */
+  Trigger AxisGreaterThan(
+      int axis, double threshold,
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * 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.
+   */
+  Trigger AxisMagnitudeGreaterThan(
+      int axis, double threshold,
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * 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
+   */
+  void SetRumble(frc::GenericHID::RumbleType type, double value);
+
+  /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  bool IsConnected() const;
+
+ private:
+  frc::GenericHID m_hid;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/CommandJoystick.h

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include <frc/Joystick.h>
+
+#include "Trigger.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/button/CommandGenericHID.h"
+
+namespace frc2 {
+/**
+ * A version of {@link frc::Joystick} with {@link Trigger} factories for
+ * command-based.
+ *
+ * @see frc::Joystick
+ */
+class CommandJoystick : public CommandGenericHID {
+ public:
+  /**
+   * Construct an instance of a controller.
+   *
+   * @param port The port index on the Driver Station that the controller is
+   * plugged into.
+   */
+  explicit CommandJoystick(int port);
+
+  /**
+   * Get the underlying GenericHID object.
+   *
+   * @return the wrapped GenericHID object
+   */
+  frc::Joystick& GetHID();
+
+  /**
+   * Constructs an event instance around the trigger button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the trigger button's digital signal
+   * attached to the given loop.
+   */
+  class Trigger Trigger(
+      frc::EventLoop* loop =
+          CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+  /**
+   * Constructs an event instance around the top button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to. Defaults to the
+   * CommandScheduler's default loop.
+   * @return an event instance representing the top button's digital signal
+   * attached to the given loop.
+   */
+  class Trigger Top(frc::EventLoop* loop = CommandScheduler::GetInstance()
+                                               .GetDefaultButtonLoop()) const;
+  /**
+   * Get the magnitude of the vector formed by the joystick's
+   * current position relative to its origin.
+   *
+   * @return The magnitude of the direction vector
+   */
+  double GetMagnitude() const;
+
+  /**
+   * Get the direction of the vector formed by the joystick and its origin. 0 is
+   * forward and clockwise is positive. (Straight right is π/2 radians or 90
+   * degrees.)
+   *
+   * @return The direction of the vector.
+   */
+  units::radian_t GetDirection() const;
+
+ private:
+  frc::Joystick m_hid;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/JoystickButton.h

@@ -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.
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Trigger.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick button presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * This class is provided by the NewCommands VendorDep
+ *
+ * @see Trigger
+ */
+class JoystickButton : public Trigger {
+ public:
+  /**
+   * Creates a JoystickButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param buttonNumber The number of the button on the joystick.
+   */
+  explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
+      : Trigger([joystick, buttonNumber] {
+          return joystick->GetRawButton(buttonNumber);
+        }) {}
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/NetworkButton.h

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "Trigger.h"
+
+namespace frc2 {
+/**
+ * A Button that uses a NetworkTable boolean field.
+ *
+ * This class is provided by the NewCommands VendorDep
+ */
+class NetworkButton : public Trigger {
+ public:
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param topic The boolean topic that contains the value.
+   */
+  explicit NetworkButton(nt::BooleanTopic topic);
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param sub The boolean subscriber that provides the value.
+   */
+  explicit NetworkButton(nt::BooleanSubscriber sub);
+
+  /**
+   * 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.
+   */
+  NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                std::string_view 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.
+   */
+  NetworkButton(std::string_view table, std::string_view 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.
+   */
+  NetworkButton(nt::NetworkTableInstance inst, std::string_view table,
+                std::string_view field);
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/POVButton.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/DriverStation.h>
+#include <frc/GenericHID.h>
+
+#include "Trigger.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick POV presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * This class is provided by the NewCommands VendorDep
+ *
+ * @see Trigger
+ */
+class POVButton : public Trigger {
+ public:
+  /**
+   * Creates a POVButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param angle The angle of the POV corresponding to a button press.
+   * @param povNumber The number of the POV on the joystick.
+   */
+  POVButton(frc::GenericHID* joystick, frc::DriverStation::POVDirection angle,
+            int povNumber = 0)
+      : Trigger([joystick, angle, povNumber] {
+          return joystick->GetPOV(povNumber) == angle;
+        }) {}
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/RobotModeTriggers.h

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc2/command/button/Trigger.h"
+
+namespace frc2 {
+
+/**
+ * A class containing static Trigger factories for running callbacks when robot
+ * mode changes.
+ */
+class RobotModeTriggers {
+ public:
+  RobotModeTriggers() = delete;
+
+  /**
+   * 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.
+   */
+  static Trigger Autonomous();
+
+  /**
+   * 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.
+   */
+  static Trigger Teleop();
+
+  /**
+   * Returns a trigger that is true when the robot is disabled.
+   *
+   * @return A trigger that is true when the robot is disabled.
+   */
+  static Trigger Disabled();
+
+  /**
+   * 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.
+   */
+  static Trigger Test();
+};
+
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/button/Trigger.h

@@ -0,0 +1,305 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <utility>
+
+#include <frc/event/BooleanEvent.h>
+#include <frc/event/EventLoop.h>
+#include <frc/filter/Debouncer.h>
+#include <units/time.h>
+#include <wpi/FunctionExtras.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+class Command;
+/**
+ * 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 #operator!}, {@link #operator||}, {@link #operator&&} operators.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+class Trigger {
+ public:
+  /**
+   * 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
+   */
+  explicit Trigger(std::function<bool()> condition)
+      : Trigger{CommandScheduler::GetInstance().GetDefaultButtonLoop(),
+                std::move(condition)} {}
+
+  /**
+   * 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
+   */
+  Trigger(frc::EventLoop* loop, std::function<bool()> condition)
+      : m_loop{loop}, m_condition{std::move(condition)} {}
+
+  /**
+   * Create a new trigger that is always `false`.
+   */
+  Trigger() : Trigger([] { return false; }) {}
+
+  Trigger(const Trigger& other);
+
+  /**
+   * Starts the command when the condition changes.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger OnChange(Command* command);
+
+  /**
+   * Starts the command when the condition changes. Moves command ownership to
+   * the button scheduler.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger OnChange(CommandPtr&& command);
+
+  /**
+   * Starts the given command whenever the condition changes from `false` to
+   * `true`.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger OnTrue(Command* command);
+
+  /**
+   * Starts the given command whenever the condition changes from `false` to
+   * `true`. Moves command ownership to the button scheduler.
+   *
+   * @param command The command to bind.
+   * @return The trigger, for chained calls.
+   */
+  Trigger OnTrue(CommandPtr&& command);
+
+  /**
+   * Starts the given command whenever the condition changes from `true` to
+   * `false`.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger OnFalse(Command* command);
+
+  /**
+   * Starts the given command whenever the condition changes from `true` to
+   * `false`.
+   *
+   * @param command The command to bind.
+   * @return The trigger, for chained calls.
+   */
+  Trigger OnFalse(CommandPtr&& command);
+
+  /**
+   * 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 RepeatCommand.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger WhileTrue(Command* command);
+
+  /**
+   * Starts the given command when the condition changes to `true` and cancels
+   * it when the condition changes to `false`. Moves command ownership to the
+   * button scheduler.
+   *
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `true`. If the command should restart, see RepeatCommand.
+   *
+   * @param command The command to bind.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhileTrue(CommandPtr&& command);
+
+  /**
+   * 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
+   * `true`. If the command should restart, see RepeatCommand.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to start
+   * @return this trigger, so calls can be chained
+   */
+  Trigger WhileFalse(Command* command);
+
+  /**
+   * Starts the given command when the condition changes to `false` and cancels
+   * it when the condition changes to `true`. Moves command ownership to the
+   * button scheduler.
+   *
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `false`. If the command should restart, see RepeatCommand.
+   *
+   * @param command The command to bind.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhileFalse(CommandPtr&& command);
+
+  /**
+   * Toggles a command when the condition changes from `false` to `true`.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to toggle
+   * @return this trigger, so calls can be chained
+   */
+  Trigger ToggleOnTrue(Command* command);
+
+  /**
+   * Toggles a command when the condition changes from `false` to `true`.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to toggle
+   * @return this trigger, so calls can be chained
+   */
+  Trigger ToggleOnTrue(CommandPtr&& command);
+
+  /**
+   * Toggles a command when the condition changes from `true` to the low
+   * state.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to toggle
+   * @return this trigger, so calls can be chained
+   */
+  Trigger ToggleOnFalse(Command* command);
+
+  /**
+   * Toggles a command when the condition changes from `true` to `false`.
+   *
+   * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
+   * lifespan of the command.
+   *
+   * @param command the command to toggle
+   * @return this trigger, so calls can be chained
+   */
+  Trigger ToggleOnFalse(CommandPtr&& command);
+
+  /**
+   * Composes two triggers with logical AND.
+   *
+   * @return A trigger which is active when both component triggers are active.
+   */
+  Trigger operator&&(std::function<bool()> rhs) {
+    return Trigger(m_loop, [condition = m_condition, rhs = std::move(rhs)] {
+      return condition() && rhs();
+    });
+  }
+
+  /**
+   * Composes two triggers with logical AND.
+   *
+   * @return A trigger which is active when both component triggers are active.
+   */
+  Trigger operator&&(Trigger rhs) {
+    return Trigger(m_loop, [condition = m_condition, rhs] {
+      return condition() && rhs.m_condition();
+    });
+  }
+
+  /**
+   * Composes two triggers with logical OR.
+   *
+   * @return A trigger which is active when either component trigger is active.
+   */
+  Trigger operator||(std::function<bool()> rhs) {
+    return Trigger(m_loop, [condition = m_condition, rhs = std::move(rhs)] {
+      return condition() || rhs();
+    });
+  }
+
+  /**
+   * Composes two triggers with logical OR.
+   *
+   * @return A trigger which is active when either component trigger is active.
+   */
+  Trigger operator||(Trigger rhs) {
+    return Trigger(m_loop, [condition = m_condition, rhs] {
+      return condition() || rhs.m_condition();
+    });
+  }
+
+  /**
+   * Composes a trigger with logical NOT.
+   *
+   * @return A trigger which is active when the component trigger is inactive,
+   * and vice-versa.
+   */
+  Trigger operator!() {
+    return Trigger(m_loop, [condition = m_condition] { return !condition(); });
+  }
+
+  /**
+   * 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 debounceTime The debounce period.
+   * @param type The debounce type.
+   * @return The debounced trigger.
+   */
+  Trigger Debounce(units::second_t debounceTime,
+                   frc::Debouncer::DebounceType type =
+                       frc::Debouncer::DebounceType::kRising);
+
+  /**
+   * Returns the current state of this trigger.
+   *
+   * @return A bool representing the current state of the trigger.
+   */
+  bool Get() const;
+
+ private:
+  /**
+   * Adds a binding to the EventLoop.
+   *
+   * @param body The body of the binding to add.
+   */
+  void AddBinding(wpi::unique_function<void(bool, bool)>&& body);
+
+  frc::EventLoop* m_loop;
+  std::function<bool()> m_condition;
+};
+}  // namespace frc2

commandsv2/src/main/native/include/frc2/command/sysid/SysIdRoutine.h

@@ -0,0 +1,198 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <string_view>
+#include <utility>
+
+#include <frc/Timer.h>
+#include <frc/sysid/SysIdRoutineLog.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Subsystem.h"
+
+namespace frc2::sysid {
+
+using ramp_rate_t = units::unit_t<
+    units::compound_unit<units::volt, units::inverse<units::second>>>;
+
+/** Hardware-independent configuration for a SysId test routine. */
+class Config {
+ public:
+  /// The voltage ramp rate used for quasistatic test routines.
+  ramp_rate_t m_rampRate{1_V / 1_s};
+
+  /// The step voltage output used for dynamic test routines.
+  units::volt_t m_stepVoltage{7_V};
+
+  /// Safety timeout for the test routine commands.
+  units::second_t m_timeout{10_s};
+
+  /// Optional handle for recording test state in a third-party logging
+  /// solution.
+  std::function<void(frc::sysid::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.
+   */
+  Config(std::optional<ramp_rate_t> rampRate,
+         std::optional<units::volt_t> stepVoltage,
+         std::optional<units::second_t> timeout,
+         std::function<void(frc::sysid::State)> recordState)
+      : m_recordState{std::move(recordState)} {
+    if (rampRate) {
+      m_rampRate = rampRate.value();
+    }
+    if (stepVoltage) {
+      m_stepVoltage = stepVoltage.value();
+    }
+    if (timeout) {
+      m_timeout = timeout.value();
+    }
+  }
+};
+
+class Mechanism {
+ public:
+  /// Sends the SysId-specified drive signal to the mechanism motors during test
+  /// routines.
+  std::function<void(units::volt_t)> m_drive;
+
+  /// Returns measured data (voltages, positions, velocities) of the mechanism
+  /// motors during test routines.
+  std::function<void(frc::sysid::SysIdRoutineLog*)> m_log;
+
+  /// The subsystem containing the motor(s) that is (or are) being
+  /// characterized.
+  frc2::Subsystem* m_subsystem;
+
+  /// 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".
+  std::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.
+   */
+  Mechanism(std::function<void(units::volt_t)> drive,
+            std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+            frc2::Subsystem* subsystem, std::string_view name)
+      : m_drive{std::move(drive)},
+        m_log{log ? std::move(log) : [](frc::sysid::SysIdRoutineLog* log) {}},
+        m_subsystem{subsystem},
+        m_name{name} {}
+
+  /**
+   * 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".
+   */
+  Mechanism(std::function<void(units::volt_t)> drive,
+            std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+            frc2::Subsystem* subsystem)
+      : m_drive{std::move(drive)},
+        m_log{log ? std::move(log) : [](frc::sysid::SysIdRoutineLog* log) {}},
+        m_subsystem{subsystem},
+        m_name{m_subsystem->GetName()} {}
+};
+
+/**
+ * Motor direction for a SysId test.
+ */
+enum Direction {
+  /// Forward.
+  kForward,
+  /// Reverse.
+  kReverse
+};
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have
+ * multiple motors.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * 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.
+ */
+class SysIdRoutine : public frc::sysid::SysIdRoutineLog {
+ public:
+  /**
+   * Create a new SysId characterization routine.
+   *
+   * @param config Hardware-independent parameters for the SysId routine.
+   * @param mechanism Hardware interface for the SysId routine.
+   */
+  SysIdRoutine(Config config, Mechanism mechanism)
+      : SysIdRoutineLog(mechanism.m_name),
+        m_config(config),
+        m_mechanism(mechanism),
+        m_recordState(config.m_recordState ? config.m_recordState
+                                           : [this](frc::sysid::State state) {
+                                               this->RecordState(state);
+                                             }) {}
+
+  frc2::CommandPtr Quasistatic(Direction direction);
+  frc2::CommandPtr Dynamic(Direction direction);
+
+ private:
+  Config m_config;
+  Mechanism m_mechanism;
+  units::volt_t m_outputVolts{0};
+  std::function<void(frc::sysid::State)> m_recordState;
+  frc::Timer timer;
+};
+}  // namespace frc2::sysid