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);
+}