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;