From e6f5c93ab154fc908db1b171b9a2b26e70aec359 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Tue, 5 Nov 2019 20:52:49 -0800
Subject: [PATCH] Clean up new C++ commands (#2027)

- Remove use of std::set. The only place std::set was actually used was in ParallelRaceGroup,
but this was of minimal utility as ParallelRaceGroup checked for duplicate subsystem
requirements, so it would be very unusual to end up with duplicate commands
in any case; replaced it with a vector.
- Remove use of std::unordered_map except for SelectCommand. Replaced with vector.
- Use pImpl idiom for CommandScheduler
- Minimize include files (remove unnecessary ones)
- Reformat include file order for consistency
---
 wpilibNewCommands/.styleguide                 |   8 +
 .../main/native/cpp/frc2/command/Command.cpp  |   2 -
 .../native/cpp/frc2/command/CommandBase.cpp   |   2 -
 .../cpp/frc2/command/CommandGroupBase.cpp     |  13 +-
 .../cpp/frc2/command/CommandScheduler.cpp     | 190 +++++++++++-------
 .../native/cpp/frc2/command/CommandState.cpp  |   2 +-
 .../cpp/frc2/command/ParallelCommandGroup.cpp |   2 +-
 .../frc2/command/ParallelDeadlineGroup.cpp    |   4 +-
 .../cpp/frc2/command/ParallelRaceGroup.cpp    |   2 +-
 .../native/cpp/frc2/command/PrintCommand.cpp  |   2 +
 .../native/cpp/frc2/command/SubsystemBase.cpp |   5 +-
 .../frc2/command/TrapezoidProfileCommand.cpp  |   2 -
 .../cpp/frc2/command/WaitUntilCommand.cpp     |   6 +-
 .../native/include/frc2/command/Command.h     |   9 +-
 .../native/include/frc2/command/CommandBase.h |   8 +-
 .../include/frc2/command/CommandGroupBase.h   |   8 +-
 .../include/frc2/command/CommandHelper.h      |   2 +-
 .../include/frc2/command/CommandScheduler.h   |  64 ++----
 .../include/frc2/command/ConditionalCommand.h |   8 +-
 .../include/frc2/command/FunctionalCommand.h  |   6 +-
 .../include/frc2/command/InstantCommand.h     |   7 +-
 .../include/frc2/command/NotifierCommand.h    |   8 +-
 .../native/include/frc2/command/PIDCommand.h  |   6 +-
 .../include/frc2/command/PIDSubsystem.h       |   3 +-
 .../frc2/command/ParallelCommandGroup.h       |   7 +-
 .../frc2/command/ParallelDeadlineGroup.h      |   7 +-
 .../include/frc2/command/ParallelRaceGroup.h  |   8 +-
 .../include/frc2/command/PerpetualCommand.h   |   6 +-
 .../include/frc2/command/PrintCommand.h       |   5 +-
 .../include/frc2/command/ProfiledPIDCommand.h |   5 +-
 .../frc2/command/ProfiledPIDSubsystem.h       |   2 +-
 .../frc2/command/ProxyScheduleCommand.h       |   7 +-
 .../include/frc2/command/RamseteCommand.h     |  21 +-
 .../native/include/frc2/command/RunCommand.h  |   7 +-
 .../include/frc2/command/ScheduleCommand.h    |   7 +-
 .../include/frc2/command/SelectCommand.h      |   9 +-
 .../frc2/command/SequentialCommandGroup.h     |   9 +-
 .../include/frc2/command/StartEndCommand.h    |   7 +-
 .../native/include/frc2/command/Subsystem.h   |   5 +-
 .../include/frc2/command/SubsystemBase.h      |   6 +-
 .../frc2/command/TrapezoidProfileCommand.h    |  13 +-
 .../native/include/frc2/command/WaitCommand.h |   7 +-
 .../include/frc2/command/WaitUntilCommand.h   |   9 +-
 43 files changed, 280 insertions(+), 236 deletions(-)
 create mode 100644 wpilibNewCommands/.styleguide

diff --git a/wpilibNewCommands/.styleguide b/wpilibNewCommands/.styleguide
new file mode 100644
index 0000000000..e75c28ca6c
--- /dev/null
+++ b/wpilibNewCommands/.styleguide
@@ -0,0 +1,8 @@
+includeOtherLibs {
+  ^HAL/
+  ^networktables/
+  ^frc/
+  ^units/
+  ^wpi/
+  ^frc2/Timer
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 692879c291..8a000cd71d 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -7,8 +7,6 @@
 
 #include "frc2/command/Command.h"
 
-#include <iostream>
-
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelCommandGroup.h"
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
index aeba26bd58..323ed6776e 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -9,8 +9,6 @@
 
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandScheduler.h>
-#include <frc2/command/SetUtilities.h>
 
 using namespace frc2;
 
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
index ba53397039..8869d4c624 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -7,19 +7,10 @@
 
 #include "frc2/command/CommandGroupBase.h"
 
-#include <set>
-
-#include "frc/WPIErrors.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
+#include <frc/WPIErrors.h>
 
 using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
-  return map.find(keyToCheck) != map.end();
-}
+
 bool CommandGroupBase::RequireUngrouped(Command& command) {
   if (command.IsGrouped()) {
     wpi_setGlobalWPIErrorWithContext(
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index 7a12a86eb7..9ac69b8704 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -7,39 +7,85 @@
 
 #include "frc2/command/CommandScheduler.h"
 
+#include <hal/HALBase.h>
 #include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandGroupBase.h>
-#include <frc2/command/Subsystem.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
 
-#include <hal/HAL.h>
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandState.h"
+#include "frc2/command/Subsystem.h"
 
 using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+  // A map from commands to their scheduling state.  Also used as a set of the
+  // currently-running commands.
+  wpi::DenseMap<Command*, CommandState> 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;
+
+  // The set of currently-registered buttons that will be polled every
+  // iteration.
+  wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+
+  bool disabled{false};
+
+  // NetworkTable entries for use in Sendable impl
+  nt::NetworkTableEntry namesEntry;
+  nt::NetworkTableEntry idsEntry;
+  nt::NetworkTableEntry cancelEntry;
+
+  // 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<Action, 4> interruptActions;
+  wpi::SmallVector<Action, 4> finishActions;
+
+  // Flag and queues for avoiding concurrent modification if commands are
+  // scheduled/canceled during run
+
+  bool inRunLoop = false;
+  wpi::DenseMap<Command*, bool> toSchedule;
+  wpi::SmallVector<Command*, 4> toCancel;
+};
+
 template <typename TMap, typename TKey>
 static bool ContainsKey(const TMap& map, TKey keyToCheck) {
   return map.find(keyToCheck) != map.end();
 }
 
-CommandScheduler::CommandScheduler() {
+CommandScheduler::CommandScheduler() : m_impl(new Impl) {
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
 }
 
+CommandScheduler::~CommandScheduler() {}
+
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;
   return scheduler;
 }
 
 void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
-  m_buttons.emplace_back(std::move(button));
+  m_impl->buttons.emplace_back(std::move(button));
 }
 
-void CommandScheduler::ClearButtons() { m_buttons.clear(); }
+void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
 
 void CommandScheduler::Schedule(bool interruptible, Command* command) {
-  if (m_inRunLoop) {
-    m_toSchedule.try_emplace(command, interruptible);
+  if (m_impl->inRunLoop) {
+    m_impl->toSchedule.try_emplace(command, interruptible);
     return;
   }
 
@@ -49,9 +95,9 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
                                "cannot be independently scheduled");
     return;
   }
-  if (m_disabled ||
+  if (m_impl->disabled ||
       (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
-      ContainsKey(m_scheduledCommands, command)) {
+      ContainsKey(m_impl->scheduledCommands, command)) {
     return;
   }
 
@@ -61,10 +107,11 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
 
   bool isDisjoint = true;
   bool allInterruptible = true;
-  for (auto&& i1 : m_requirements) {
+  for (auto&& i1 : m_impl->requirements) {
     if (requirements.find(i1.first) != requirements.end()) {
       isDisjoint = false;
-      allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
+      allInterruptible &=
+          m_impl->scheduledCommands[i1.second].IsInterruptible();
       intersection.emplace_back(i1.second);
     }
   }
@@ -76,12 +123,12 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
       }
     }
     command->Initialize();
-    m_scheduledCommands[command] = CommandState{interruptible};
-    for (auto&& action : m_initActions) {
+    m_impl->scheduledCommands[command] = CommandState{interruptible};
+    for (auto&& action : m_impl->initActions) {
       action(*command);
     }
     for (auto&& requirement : requirements) {
-      m_requirements[requirement] = command;
+      m_impl->requirements[requirement] = command;
     }
   }
 }
@@ -115,24 +162,24 @@ void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::Run() {
-  if (m_disabled) {
+  if (m_impl->disabled) {
     return;
   }
 
   // Run the periodic method of all registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
+  for (auto&& subsystem : m_impl->subsystems) {
     subsystem.getFirst()->Periodic();
   }
 
   // Poll buttons for new commands to add.
-  for (auto&& button : m_buttons) {
+  for (auto&& button : m_impl->buttons) {
     button();
   }
 
-  m_inRunLoop = true;
+  m_impl->inRunLoop = true;
   // Run scheduled commands, remove finished commands.
-  for (auto iterator = m_scheduledCommands.begin();
-       iterator != m_scheduledCommands.end(); iterator++) {
+  for (auto iterator = m_impl->scheduledCommands.begin();
+       iterator != m_impl->scheduledCommands.end(); iterator++) {
     Command* command = iterator->getFirst();
 
     if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
@@ -141,53 +188,53 @@ void CommandScheduler::Run() {
     }
 
     command->Execute();
-    for (auto&& action : m_executeActions) {
+    for (auto&& action : m_impl->executeActions) {
       action(*command);
     }
 
     if (command->IsFinished()) {
       command->End(false);
-      for (auto&& action : m_finishActions) {
+      for (auto&& action : m_impl->finishActions) {
         action(*command);
       }
 
       for (auto&& requirement : command->GetRequirements()) {
-        m_requirements.erase(requirement);
+        m_impl->requirements.erase(requirement);
       }
 
-      m_scheduledCommands.erase(iterator);
+      m_impl->scheduledCommands.erase(iterator);
     }
   }
-  m_inRunLoop = false;
+  m_impl->inRunLoop = false;
 
-  for (auto&& commandInterruptible : m_toSchedule) {
+  for (auto&& commandInterruptible : m_impl->toSchedule) {
     Schedule(commandInterruptible.second, commandInterruptible.first);
   }
 
-  for (auto&& command : m_toCancel) {
+  for (auto&& command : m_impl->toCancel) {
     Cancel(command);
   }
 
-  m_toSchedule.clear();
-  m_toCancel.clear();
+  m_impl->toSchedule.clear();
+  m_impl->toCancel.clear();
 
   // Add default commands for un-required registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
-    auto s = m_requirements.find(subsystem.getFirst());
-    if (s == m_requirements.end() && subsystem.getSecond()) {
+  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()});
     }
   }
 }
 
 void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
-  m_subsystems[subsystem] = nullptr;
+  m_impl->subsystems[subsystem] = nullptr;
 }
 
 void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
-  auto s = m_subsystems.find(subsystem);
-  if (s != m_subsystems.end()) {
-    m_subsystems.erase(s);
+  auto s = m_impl->subsystems.find(subsystem);
+  if (s != m_impl->subsystems.end()) {
+    m_impl->subsystems.erase(s);
   }
 }
 
@@ -206,8 +253,8 @@ void CommandScheduler::UnregisterSubsystem(
 }
 
 Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
-  auto&& find = m_subsystems.find(subsystem);
-  if (find != m_subsystems.end()) {
+  auto&& find = m_impl->subsystems.find(subsystem);
+  if (find != m_impl->subsystems.end()) {
     return find->second.get();
   } else {
     return nullptr;
@@ -215,21 +262,21 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
 }
 
 void CommandScheduler::Cancel(Command* command) {
-  if (m_inRunLoop) {
-    m_toCancel.emplace_back(command);
+  if (m_impl->inRunLoop) {
+    m_impl->toCancel.emplace_back(command);
     return;
   }
 
-  auto find = m_scheduledCommands.find(command);
-  if (find == m_scheduledCommands.end()) return;
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find == m_impl->scheduledCommands.end()) return;
   command->End(true);
-  for (auto&& action : m_interruptActions) {
+  for (auto&& action : m_impl->interruptActions) {
     action(*command);
   }
-  m_scheduledCommands.erase(find);
-  for (auto&& requirement : m_requirements) {
+  m_impl->scheduledCommands.erase(find);
+  for (auto&& requirement : m_impl->requirements) {
     if (requirement.second == command) {
-      m_requirements.erase(requirement.first);
+      m_impl->requirements.erase(requirement.first);
     }
   }
 }
@@ -247,14 +294,14 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::CancelAll() {
-  for (auto&& command : m_scheduledCommands) {
+  for (auto&& command : m_impl->scheduledCommands) {
     Cancel(command.first);
   }
 }
 
 double CommandScheduler::TimeSinceScheduled(const Command* command) const {
-  auto find = m_scheduledCommands.find(command);
-  if (find != m_scheduledCommands.end()) {
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find != m_impl->scheduledCommands.end()) {
     return find->second.TimeSinceInitialized();
   } else {
     return -1;
@@ -281,65 +328,72 @@ bool CommandScheduler::IsScheduled(
 }
 
 bool CommandScheduler::IsScheduled(const Command* command) const {
-  return m_scheduledCommands.find(command) != m_scheduledCommands.end();
+  return m_impl->scheduledCommands.find(command) !=
+         m_impl->scheduledCommands.end();
 }
 
 Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
-  auto find = m_requirements.find(subsystem);
-  if (find != m_requirements.end()) {
+  auto find = m_impl->requirements.find(subsystem);
+  if (find != m_impl->requirements.end()) {
     return find->second;
   } else {
     return nullptr;
   }
 }
 
-void CommandScheduler::Disable() { m_disabled = true; }
+void CommandScheduler::Disable() { m_impl->disabled = true; }
 
-void CommandScheduler::Enable() { m_disabled = false; }
+void CommandScheduler::Enable() { m_impl->disabled = false; }
 
 void CommandScheduler::OnCommandInitialize(Action action) {
-  m_initActions.emplace_back(std::move(action));
+  m_impl->initActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandExecute(Action action) {
-  m_executeActions.emplace_back(std::move(action));
+  m_impl->executeActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandInterrupt(Action action) {
-  m_interruptActions.emplace_back(std::move(action));
+  m_impl->interruptActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandFinish(Action action) {
-  m_finishActions.emplace_back(std::move(action));
+  m_impl->finishActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
-  m_namesEntry = builder.GetEntry("Names");
-  m_idsEntry = builder.GetEntry("Ids");
-  m_cancelEntry = builder.GetEntry("Cancel");
+  m_impl->namesEntry = builder.GetEntry("Names");
+  m_impl->idsEntry = builder.GetEntry("Ids");
+  m_impl->cancelEntry = builder.GetEntry("Cancel");
 
   builder.SetUpdateTable([this] {
     double tmp[1];
     tmp[0] = 0;
-    auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
+    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
     for (auto cancel : toCancel) {
       uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
       Command* command = reinterpret_cast<Command*>(ptrTmp);
-      if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
+      if (m_impl->scheduledCommands.find(command) !=
+          m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
     }
 
     wpi::SmallVector<std::string, 8> names;
     wpi::SmallVector<double, 8> ids;
-    for (auto&& command : m_scheduledCommands) {
+    for (auto&& command : m_impl->scheduledCommands) {
       names.emplace_back(command.first->GetName());
       uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
-    m_namesEntry.SetStringArray(names);
-    m_idsEntry.SetDoubleArray(ids);
+    m_impl->namesEntry.SetStringArray(names);
+    m_impl->idsEntry.SetDoubleArray(ids);
   });
 }
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+                                             std::unique_ptr<Command> command) {
+  m_impl->subsystems[subsystem] = std::move(command);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
index 78ae006e64..b41fa70e30 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
@@ -7,7 +7,7 @@
 
 #include "frc2/command/CommandState.h"
 
-#include "frc/Timer.h"
+#include <frc/Timer.h>
 
 using namespace frc2;
 CommandState::CommandState(bool interruptible)
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
index d8a4159d23..557984e43f 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -72,7 +72,7 @@ void ParallelCommandGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
+      m_commands.emplace_back(std::move(command), false);
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
index d967e67b48..910bc8ce1a 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -67,7 +67,7 @@ void ParallelDeadlineGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
+      m_commands.emplace_back(std::move(command), false);
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "
@@ -80,7 +80,7 @@ void ParallelDeadlineGroup::AddCommands(
 void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
   m_deadline = deadline.get();
   m_deadline->SetGrouped(true);
-  m_commands[std::move(deadline)] = false;
+  m_commands.emplace_back(std::move(deadline), false);
   AddRequirements(m_deadline->GetRequirements());
   m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
 }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index 8a027172fd..466cc79ea6 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -58,7 +58,7 @@ void ParallelRaceGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands.emplace(std::move(command));
+      m_commands.emplace_back(std::move(command));
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
index 8bd62ea0ff..e077760988 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
@@ -7,6 +7,8 @@
 
 #include "frc2/command/PrintCommand.h"
 
+#include <wpi/raw_ostream.h>
+
 using namespace frc2;
 
 PrintCommand::PrintCommand(const wpi::Twine& message)
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 226f080655..5e5ecbfe33 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -9,8 +9,9 @@
 
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
 
 using namespace frc2;
 
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
index bb17edc24e..08628d2fed 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
@@ -7,8 +7,6 @@
 
 #include "frc2/command/TrapezoidProfileCommand.h"
 
-#include <units/units.h>
-
 using namespace frc2;
 
 TrapezoidProfileCommand::TrapezoidProfileCommand(
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
index d7a0daf374..8479581351 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
@@ -7,13 +7,15 @@
 
 #include "frc2/command/WaitUntilCommand.h"
 
+#include <frc2/Timer.h>
+
 using namespace frc2;
 
 WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
     : m_condition{std::move(condition)} {}
 
-WaitUntilCommand::WaitUntilCommand(double time)
-    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+    : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
 
 bool WaitUntilCommand::IsFinished() { return m_condition(); }
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 49e904e41e..dc26b8a2e1 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -7,18 +7,17 @@
 
 #pragma once
 
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <frc2/command/Subsystem.h>
-
+#include <functional>
 #include <memory>
 #include <string>
 
+#include <frc/ErrorBase.h>
 #include <units/units.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
+
+#include "frc2/command/Subsystem.h"
 
 namespace frc2 {
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
index 3b1698f4fd..81af2f7865 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -7,15 +7,15 @@
 
 #pragma once
 
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
+#include <initializer_list>
 #include <string>
 
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
 #include <wpi/SmallSet.h>
 #include <wpi/Twine.h>
 
-#include "Command.h"
+#include "frc2/command/Command.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
index 9d265b493e..8b04b4c8bd 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
@@ -7,13 +7,13 @@
 
 #pragma once
 
-#include <frc/ErrorBase.h>
-
+#include <initializer_list>
 #include <memory>
-#include <set>
 #include <vector>
 
-#include "CommandBase.h"
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
 
 namespace frc2 {
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
index ec15f88020..f78a848d10 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -11,7 +11,7 @@
 #include <type_traits>
 #include <utility>
 
-#include "Command.h"
+#include "frc2/command/Command.h"
 
 namespace frc2 {
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index 2e9cdd5b26..c114d6a509 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -7,22 +7,16 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <memory>
+#include <utility>
+
 #include <frc/ErrorBase.h>
-#include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/DenseMap.h>
+#include <wpi/ArrayRef.h>
 #include <wpi/FunctionExtras.h>
-#include <wpi/SmallSet.h>
-
-#include "CommandState.h"
 
 namespace frc2 {
 class Command;
@@ -46,6 +40,10 @@ class CommandScheduler final : public frc::Sendable,
    */
   static CommandScheduler& GetInstance();
 
+  ~CommandScheduler();
+  CommandScheduler(const CommandScheduler&) = delete;
+  CommandScheduler& operator=(const CommandScheduler&) = delete;
+
   using Action = std::function<void(const Command&)>;
 
   /**
@@ -186,8 +184,9 @@ class CommandScheduler final : public frc::Sendable,
                                  "Default commands should not end!");
       return;
     }
-    m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
-        std::forward<T>(defaultCommand));
+    SetDefaultCommandImpl(subsystem,
+                          std::make_unique<std::remove_reference_t<T>>(
+                              std::forward<T>(defaultCommand)));
   }
 
   /**
@@ -330,42 +329,11 @@ class CommandScheduler final : public frc::Sendable,
   // Constructor; private as this is a singleton
   CommandScheduler();
 
-  // A map from commands to their scheduling state.  Also used as a set of the
-  // currently-running commands.
-  wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
+  void SetDefaultCommandImpl(Subsystem* subsystem,
+                             std::unique_ptr<Command> command);
 
-  // A map from required subsystems to their requiring commands.  Also used as a
-  // set of the currently-required subsystems.
-  wpi::DenseMap<Subsystem*, Command*> m_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>> m_subsystems;
-
-  // The set of currently-registered buttons that will be polled every
-  // iteration.
-  wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
-
-  bool m_disabled{false};
-
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry m_namesEntry;
-  nt::NetworkTableEntry m_idsEntry;
-  nt::NetworkTableEntry m_cancelEntry;
-
-  // Lists of user-supplied actions to be executed on scheduling events for
-  // every command.
-  wpi::SmallVector<Action, 4> m_initActions;
-  wpi::SmallVector<Action, 4> m_executeActions;
-  wpi::SmallVector<Action, 4> m_interruptActions;
-  wpi::SmallVector<Action, 4> m_finishActions;
-
-  // Flag and queues for avoiding concurrent modification if commands are
-  // scheduled/canceled during run
-
-  bool m_inRunLoop = false;
-  wpi::DenseMap<Command*, bool> m_toSchedule;
-  wpi::SmallVector<Command*, 4> m_toCancel;
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
 
   friend class CommandTestBase;
 };
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
index 0419cf97ec..af9f11396b 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
@@ -7,13 +7,13 @@
 
 #pragma once
 
-#include <iostream>
+#include <functional>
 #include <memory>
 #include <utility>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
index b47135c52f..dbec995ed4 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -7,8 +7,10 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
index ec28a11023..b9d54c7aa9 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
index 3365c0445e..9a3ee26634 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -7,12 +7,14 @@
 
 #pragma once
 
-#include <frc/Notifier.h>
+#include <functional>
+#include <initializer_list>
 
+#include <frc/Notifier.h>
 #include <units/units.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index 4a13f3dd3c..15563710de 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -7,7 +7,11 @@
 
 #pragma once
 
-#include "frc/controller/PIDController.h"
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/PIDController.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 7aa21c00fa..9b1587d042 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include "frc/controller/PIDController.h"
+#include <frc/controller/PIDController.h>
+
 #include "frc2/command/SubsystemBase.h"
 
 namespace frc2 {
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
index 322e7b1a83..5b0f0da8b7 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -13,12 +13,11 @@
 #endif
 
 #include <memory>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -89,7 +88,7 @@ class ParallelCommandGroup
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
 
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
   bool m_runWhenDisabled{true};
   bool isRunning = false;
 };
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
index 168d0f8525..1606548475 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -13,12 +13,11 @@
 #endif
 
 #include <memory>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -99,7 +98,7 @@ class ParallelDeadlineGroup
 
   void SetDeadline(std::unique_ptr<Command>&& deadline);
 
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
   Command* m_deadline;
   bool m_runWhenDisabled{true};
   bool isRunning = false;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
index d7411e9cbe..167dee4ae0 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -13,13 +13,11 @@
 #endif
 
 #include <memory>
-#include <set>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -78,7 +76,7 @@ class ParallelRaceGroup
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
 
-  std::set<std::unique_ptr<Command>> m_commands;
+  std::vector<std::unique_ptr<Command>> m_commands;
   bool m_runWhenDisabled{true};
   bool m_finished{false};
   bool isRunning = false;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
index 3f3c9e7b79..4a937045bb 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
@@ -15,9 +15,9 @@
 #include <memory>
 #include <utility>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
index fb420cb179..7b706a0554 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
@@ -8,10 +8,9 @@
 #pragma once
 
 #include <wpi/Twine.h>
-#include <wpi/raw_ostream.h>
 
-#include "CommandHelper.h"
-#include "InstantCommand.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/InstantCommand.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 4e46e5395e..0378ab7cbe 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
 
-#include "frc/controller/ProfiledPIDController.h"
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index 4fa47b247d..5b2aaf7e15 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -7,9 +7,9 @@
 
 #pragma once
 
+#include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
 
-#include "frc/controller/ProfiledPIDController.h"
 #include "frc2/command/SubsystemBase.h"
 
 namespace frc2 {
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
index 8906424d7b..ef5204f200 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
@@ -7,11 +7,12 @@
 
 #pragma once
 
+#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
index 82f7835543..e717250e7c 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -5,21 +5,22 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#pragma once
+
 #include <functional>
+#include <initializer_list>
 #include <memory>
 
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc2/Timer.h>
 #include <units/units.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc/controller/PIDController.h"
-#include "frc/controller/RamseteController.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc2/Timer.h"
-
-#pragma once
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
index 5a0524ad91..56cf754934 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
index 422823b349..c2824ca5bd 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -7,11 +7,12 @@
 
 #pragma once
 
+#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
index 8dba378d12..a5d3b56ef8 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -15,14 +15,14 @@
 #include <memory>
 #include <unordered_map>
 #include <utility>
+#include <type_traits>
 #include <vector>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
 
 namespace frc2 {
-template <typename Key>
 /**
  * Runs one of a selection of commands, either using a selector and a key to
  * command mapping, or a supplier that returns the command directly at runtime.
@@ -39,6 +39,7 @@ template <typename Key>
  * <p>As a rule, CommandGroups require the union of the requirements of their
  * component commands.
  */
+template <typename Key>
 class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
  public:
   /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
index dd1f2ce47f..c49356eea7 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -14,15 +14,16 @@
 
 #include <limits>
 #include <memory>
+#include <type_traits>
 #include <utility>
 #include <vector>
 
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
 #include <wpi/ArrayRef.h>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
index 987b9dbbdf..8c7d64d2a5 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index 5eb10bc302..687510d588 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -7,10 +7,11 @@
 
 #pragma once
 
-#include <frc2/command/CommandScheduler.h>
-
+#include <type_traits>
 #include <utility>
 
+#include "frc2/command/CommandScheduler.h"
+
 namespace frc2 {
 class Command;
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
index e7dffc7d27..d75ad0d852 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -7,12 +7,12 @@
 
 #pragma once
 
+#include <string>
+
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
 
-#include <string>
-
-#include "Subsystem.h"
+#include "frc2/command/Subsystem.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 6864f53f26..90ec8b5de3 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -5,15 +5,16 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <frc2/Timer.h>
 
-#include <functional>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-#pragma once
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index 15445922b1..412647659b 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -7,12 +7,11 @@
 
 #pragma once
 
+#include <frc2/Timer.h>
 #include <units/units.h>
-#include <wpi/Twine.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
index 8da18e80c6..50b4855f6f 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "frc/Timer.h"
+#include <functional>
+
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
@@ -36,7 +39,7 @@ class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
    *
    * @param time the match time after which to end, in seconds
    */
-  explicit WaitUntilCommand(double time);
+  explicit WaitUntilCommand(units::second_t time);
 
   WaitUntilCommand(WaitUntilCommand&& other) = default;