Add new C++ Command framework (#1785)

This is the C++ version of #1682.

The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the frc2 namespace.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main frc namespace.

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

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Command.h"
+
+#include <iostream>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
+
+void Command::Initialize() {}
+void Command::Execute() {}
+void Command::End(bool interrupted) {}
+
+ParallelRaceGroup Command::WithTimeout(double seconds) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitCommand>(seconds));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return ParallelRaceGroup(std::move(temp));
+}
+
+ParallelRaceGroup Command::InterruptOn(std::function<bool()> condition) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return ParallelRaceGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<InstantCommand>(
+      std::move(toRun), std::initializer_list<Subsystem*>{}));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::WhenFinished(std::function<void()> toRun) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  temp.emplace_back(std::make_unique<InstantCommand>(
+      std::move(toRun), std::initializer_list<Subsystem*>{}));
+  return SequentialCommandGroup(std::move(temp));
+}
+
+PerpetualCommand Command::Perpetually() && {
+  return PerpetualCommand(std::move(*this).TransferOwnership());
+}
+
+ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
+
+void Command::Schedule(bool interruptible) {
+  CommandScheduler::GetInstance().Schedule(interruptible, this);
+}
+
+void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
+
+bool Command::IsScheduled() const {
+  return CommandScheduler::GetInstance().IsScheduled(this);
+}
+
+bool Command::HasRequirement(Subsystem* requirement) const {
+  bool hasRequirement = false;
+  for (auto&& subsystem : GetRequirements()) {
+    hasRequirement |= requirement == subsystem;
+  }
+  return hasRequirement;
+}
+
+std::string Command::GetName() const { return GetTypeName(*this); }
+
+bool Command::IsGrouped() const { return m_isGrouped; }
+
+void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
+
+namespace frc2 {
+bool RequirementsDisjoint(Command* first, Command* second) {
+  bool disjoint = true;
+  auto&& requirements = second->GetRequirements();
+  for (auto&& requirement : first->GetRequirements()) {
+    disjoint &= requirements.find(requirement) == requirements.end();
+  }
+  return disjoint;
+}
+}  // namespace frc2

wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc2/command/CommandScheduler.h>
+#include <frc2/command/SetUtilities.h>
+
+using namespace frc2;
+
+CommandBase::CommandBase() {
+  m_name = Command::GetName();
+  m_subsystem = "Unknown";
+}
+
+CommandBase::CommandBase(const CommandBase& other) : Sendable{}, Command{} {
+  m_name = other.m_name;
+  m_subsystem = other.m_subsystem;
+  m_requirements = other.m_requirements;
+}
+
+void CommandBase::AddRequirements(
+    std::initializer_list<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
+  return m_requirements;
+}
+
+void CommandBase::SetName(const wpi::Twine& name) { m_name = name.str(); }
+
+std::string CommandBase::GetName() const { return m_name; }
+
+std::string CommandBase::GetSubsystem() const { return m_subsystem; }
+
+void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
+  m_subsystem = subsystem.str();
+}
+
+void CommandBase::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
+  builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
+                             [this](bool value) {
+                               bool isScheduled = IsScheduled();
+                               if (value && !isScheduled) {
+                                 Schedule();
+                               } else if (!value && isScheduled) {
+                                 Cancel();
+                               }
+                             });
+}

wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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"
+
+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(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+    return false;
+  } else {
+    return true;
+  }
+}
+
+bool CommandGroupBase::RequireUngrouped(
+    wpi::ArrayRef<std::unique_ptr<Command>> commands) {
+  bool allUngrouped = true;
+  for (auto&& command : commands) {
+    allUngrouped &= !command.get()->IsGrouped();
+  }
+  if (!allUngrouped) {
+    wpi_setGlobalWPIErrorWithContext(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+  }
+  return allUngrouped;
+}
+
+bool CommandGroupBase::RequireUngrouped(
+    std::initializer_list<Command*> commands) {
+  bool allUngrouped = true;
+  for (auto&& command : commands) {
+    allUngrouped &= !command->IsGrouped();
+  }
+  if (!allUngrouped) {
+    wpi_setGlobalWPIErrorWithContext(
+        CommandIllegalUse,
+        "Commands cannot be added to more than one CommandGroup");
+  }
+  return allUngrouped;
+}

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

@@ -0,0 +1,320 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc2/command/CommandGroupBase.h>
+#include <frc2/command/Subsystem.h>
+
+#include <hal/HAL.h>
+
+using namespace frc2;
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+  return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() { SetName("Scheduler"); }
+
+CommandScheduler& CommandScheduler::GetInstance() {
+  static CommandScheduler scheduler;
+  return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+  m_buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+  if (command->IsGrouped()) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "A command that is part of a command group "
+                               "cannot be independently scheduled");
+    return;
+  }
+  if (m_disabled ||
+      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+      ContainsKey(m_scheduledCommands, command)) {
+    return;
+  }
+
+  const auto& requirements = command->GetRequirements();
+
+  wpi::SmallVector<Command*, 8> intersection;
+
+  bool isDisjoint = true;
+  bool allInterruptible = true;
+  for (auto&& i1 : m_requirements) {
+    if (requirements.find(i1.first) != requirements.end()) {
+      isDisjoint = false;
+      allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
+      intersection.emplace_back(i1.second);
+    }
+  }
+
+  if (isDisjoint || allInterruptible) {
+    if (allInterruptible) {
+      for (auto&& cmdToCancel : intersection) {
+        Cancel(cmdToCancel);
+      }
+    }
+    command->Initialize();
+    m_scheduledCommands[command] = CommandState{interruptible};
+    for (auto&& action : m_initActions) {
+      action(*command);
+    }
+    for (auto&& requirement : requirements) {
+      m_requirements[requirement] = command;
+    }
+  }
+}
+
+void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+
+void CommandScheduler::Schedule(bool interruptible,
+                                wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(interruptible, command);
+  }
+}
+
+void CommandScheduler::Schedule(bool interruptible,
+                                std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(interruptible, command);
+  }
+}
+
+void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(true, command);
+  }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Schedule(true, command);
+  }
+}
+
+void CommandScheduler::Run() {
+  if (m_disabled) {
+    return;
+  }
+
+  // Run the periodic method of all registered subsystems.
+  for (auto&& subsystem : m_subsystems) {
+    subsystem.getFirst()->Periodic();
+  }
+
+  // Poll buttons for new commands to add.
+  for (auto&& button : m_buttons) {
+    button();
+  }
+
+  // Run scheduled commands, remove finished commands.
+  for (auto iterator = m_scheduledCommands.begin();
+       iterator != m_scheduledCommands.end(); iterator++) {
+    Command* command = iterator->getFirst();
+
+    if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+      Cancel(command);
+      continue;
+    }
+
+    command->Execute();
+    for (auto&& action : m_executeActions) {
+      action(*command);
+    }
+
+    if (command->IsFinished()) {
+      command->End(false);
+      for (auto&& action : m_finishActions) {
+        action(*command);
+      }
+
+      for (auto&& requirement : command->GetRequirements()) {
+        m_requirements.erase(requirement);
+      }
+
+      m_scheduledCommands.erase(iterator);
+    }
+  }
+
+  // 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()) {
+      Schedule({subsystem.getSecond().get()});
+    }
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+  m_subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+  auto s = m_subsystems.find(subsystem);
+  if (s != m_subsystems.end()) {
+    m_subsystems.erase(s);
+  }
+}
+
+void CommandScheduler::RegisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+    std::initializer_list<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+  auto&& find = m_subsystems.find(subsystem);
+  if (find != m_subsystems.end()) {
+    return find->second.get();
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+  auto find = m_scheduledCommands.find(command);
+  if (find == m_scheduledCommands.end()) return;
+  command->End(true);
+  for (auto&& action : m_interruptActions) {
+    action(*command);
+  }
+  m_scheduledCommands.erase(find);
+  for (auto&& requirement : m_requirements) {
+    if (requirement.second == command) {
+      m_requirements.erase(requirement.first);
+    }
+  }
+}
+
+void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+  for (auto command : commands) {
+    Cancel(command);
+  }
+}
+
+void CommandScheduler::CancelAll() {
+  for (auto&& command : m_scheduledCommands) {
+    Cancel(command.first);
+  }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+  auto find = m_scheduledCommands.find(command);
+  if (find != m_scheduledCommands.end()) {
+    return find->second.TimeSinceInitialized();
+  } else {
+    return -1;
+  }
+}
+bool CommandScheduler::IsScheduled(
+    wpi::ArrayRef<const Command*> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(
+    std::initializer_list<const Command*> commands) const {
+  for (auto command : commands) {
+    if (!IsScheduled(command)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+  return m_scheduledCommands.find(command) != m_scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+  auto find = m_requirements.find(subsystem);
+  if (find != m_requirements.end()) {
+    return find->second;
+  } else {
+    return nullptr;
+  }
+}
+
+void CommandScheduler::Disable() { m_disabled = true; }
+
+void CommandScheduler::Enable() { m_disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+  m_initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+  m_executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+  m_interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+  m_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");
+
+  builder.SetUpdateTable([this] {
+    double tmp[1];
+    tmp[0] = 0;
+    auto toCancel = m_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()) {
+        Cancel(command);
+      }
+      m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+    }
+
+    wpi::SmallVector<std::string, 8> names;
+    wpi::SmallVector<double, 8> ids;
+    for (auto&& command : m_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);
+  });
+}

wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandState.h"
+
+#include "frc/Timer.h"
+
+using namespace frc2;
+CommandState::CommandState(bool interruptible)
+    : m_interruptible{interruptible} {
+  StartTiming();
+  StartRunning();
+}
+
+void CommandState::StartTiming() {
+  m_startTime = frc::Timer::GetFPGATimestamp();
+}
+void CommandState::StartRunning() { m_startTime = -1; }
+double CommandState::TimeSinceInitialized() const {
+  return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
+}

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ConditionalCommand.h"
+
+using namespace frc2;
+
+ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                                       std::unique_ptr<Command>&& onFalse,
+                                       std::function<bool()> condition)
+    : m_condition{std::move(condition)} {
+  if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
+    return;
+  }
+
+  m_onTrue = std::move(onTrue);
+  m_onFalse = std::move(onFalse);
+
+  m_onTrue->SetGrouped(true);
+  m_onFalse->SetGrouped(true);
+
+  m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
+  m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
+
+  AddRequirements(m_onTrue->GetRequirements());
+  AddRequirements(m_onFalse->GetRequirements());
+}
+
+void ConditionalCommand::Initialize() {
+  if (m_condition()) {
+    m_selectedCommand = m_onTrue.get();
+  } else {
+    m_selectedCommand = m_onFalse.get();
+  }
+  m_selectedCommand->Initialize();
+}
+
+void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
+
+void ConditionalCommand::End(bool interrupted) {
+  m_selectedCommand->End(interrupted);
+}
+
+bool ConditionalCommand::IsFinished() {
+  return m_selectedCommand->IsFinished();
+}
+
+bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+                                     std::function<void()> onExecute,
+                                     std::function<void(bool)> onEnd,
+                                     std::function<bool()> isFinished)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {}
+
+void FunctionalCommand::Initialize() { m_onInit(); }
+
+void FunctionalCommand::Execute() { m_onExecute(); }
+
+void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+
+bool FunctionalCommand::IsFinished() { return m_isFinished(); }

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

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+
+InstantCommand::InstantCommand(std::function<void()> toRun,
+                               std::initializer_list<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+InstantCommand::InstantCommand() : m_toRun{[] {}} {}
+
+void InstantCommand::Initialize() { m_toRun(); }
+
+bool InstantCommand::IsFinished() { return true; }

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

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+
+NotifierCommand::NotifierCommand(std::function<void()> toRun, double period,
+                                 std::initializer_list<Subsystem*> requirements)
+    : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+  AddRequirements(requirements);
+}
+
+NotifierCommand::NotifierCommand(NotifierCommand&& other)
+    : CommandHelper(std::move(other)),
+      m_toRun(other.m_toRun),
+      m_notifier(other.m_toRun),
+      m_period(other.m_period) {}
+
+NotifierCommand::NotifierCommand(const NotifierCommand& other)
+    : CommandHelper(other),
+      m_toRun(other.m_toRun),
+      m_notifier(frc::Notifier(other.m_toRun)),
+      m_period(other.m_period) {}
+
+void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
+
+void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }

wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDCommand.h"
+
+using namespace frc2;
+
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       std::function<double()> setpointSource,
+                       std::function<void(double)> useOutput,
+                       std::initializer_list<Subsystem*> requirements)
+    : m_controller{controller},
+      m_measurement{std::move(measurementSource)},
+      m_setpoint{std::move(setpointSource)},
+      m_useOutput{std::move(useOutput)} {
+  AddRequirements(requirements);
+}
+
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       double setpoint, std::function<void(double)> useOutput,
+                       std::initializer_list<Subsystem*> requirements)
+    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+                 useOutput, requirements) {}
+
+void PIDCommand::Initialize() { m_controller.Reset(); }
+
+void PIDCommand::Execute() {
+  m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
+}
+
+void PIDCommand::End(bool interrupted) { m_useOutput(0); }
+
+void PIDCommand::SetOutput(std::function<void(double)> useOutput) {
+  m_useOutput = useOutput;
+}
+
+void PIDCommand::SetSetpoint(std::function<double()> setpointSource) {
+  m_setpoint = setpointSource;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+  m_setpoint = [setpoint] { return setpoint; };
+}
+
+void PIDCommand::SetSetpointRelative(double relativeSetpoint) {
+  SetSetpoint(m_setpoint() + relativeSetpoint);
+}
+
+PIDController& PIDCommand::getController() { return m_controller; }

wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDSubsystem.h"
+
+using namespace frc2;
+
+PIDSubsystem::PIDSubsystem(PIDController controller)
+    : m_controller{controller} {}
+
+void PIDSubsystem::Periodic() {
+  if (m_enabled) {
+    UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+  }
+}
+
+void PIDSubsystem::Enable() {
+  m_controller.Reset();
+  m_enabled = true;
+}
+
+void PIDSubsystem::Disable() {
+  UseOutput(0);
+  m_enabled = false;
+}
+
+PIDController& PIDSubsystem::GetController() { return m_controller; }

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

@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelCommandGroup.h"
+
+using namespace frc2;
+
+ParallelCommandGroup::ParallelCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelCommandGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  isRunning = true;
+}
+
+void ParallelCommandGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) continue;
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+    }
+  }
+}
+
+void ParallelCommandGroup::End(bool interrupted) {
+  if (interrupted) {
+    for (auto& commandRunning : m_commands) {
+      if (commandRunning.second) {
+        commandRunning.first->End(true);
+      }
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelCommandGroup::IsFinished() {
+  for (auto& command : m_commands) {
+    if (command.second) return false;
+  }
+  return true;
+}
+
+bool ParallelCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void ParallelCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  for (auto&& command : commands) {
+    if (!RequireUngrouped(*command)) return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands[std::move(command)] = false;
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}

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

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelDeadlineGroup.h"
+
+using namespace frc2;
+
+ParallelDeadlineGroup::ParallelDeadlineGroup(
+    std::unique_ptr<Command>&& deadline,
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  SetDeadline(std::move(deadline));
+  AddCommands(std::move(commands));
+}
+
+void ParallelDeadlineGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning.first->Initialize();
+    commandRunning.second = true;
+  }
+  isRunning = true;
+}
+
+void ParallelDeadlineGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning.second) continue;
+    commandRunning.first->Execute();
+    if (commandRunning.first->IsFinished()) {
+      commandRunning.first->End(false);
+      commandRunning.second = false;
+    }
+  }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    if (commandRunning.second) {
+      commandRunning.first->End(true);
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands[std::move(command)] = false;
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}
+
+void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
+  m_deadline = deadline.get();
+  m_deadline->SetGrouped(true);
+  m_commands[std::move(deadline)] = false;
+  AddRequirements(m_deadline->GetRequirements());
+  m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}

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

@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelRaceGroup.h"
+
+using namespace frc2;
+
+ParallelRaceGroup::ParallelRaceGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void ParallelRaceGroup::Initialize() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Initialize();
+  }
+  isRunning = true;
+}
+
+void ParallelRaceGroup::Execute() {
+  for (auto& commandRunning : m_commands) {
+    commandRunning->Execute();
+    if (commandRunning->IsFinished()) {
+      m_finished = true;
+      commandRunning->End(false);
+    }
+  }
+}
+
+void ParallelRaceGroup::End(bool interrupted) {
+  for (auto& commandRunning : m_commands) {
+    if (!commandRunning->IsFinished()) {
+      commandRunning->End(true);
+    }
+  }
+  isRunning = false;
+}
+
+bool ParallelRaceGroup::IsFinished() { return m_finished; }
+
+bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
+
+void ParallelRaceGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (isRunning) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    if (RequirementsDisjoint(this, command.get())) {
+      command->SetGrouped(true);
+      AddRequirements(command->GetRequirements());
+      m_runWhenDisabled &= command->RunsWhenDisabled();
+      m_commands.emplace(std::move(command));
+    } else {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Multiple commands in a parallel group cannot "
+                                 "require the same subsystems");
+      return;
+    }
+  }
+}

wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+
+PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
+  if (!CommandGroupBase::RequireUngrouped(command)) {
+    return;
+  }
+  m_command = std::move(command);
+  m_command->SetGrouped(true);
+  AddRequirements(m_command->GetRequirements());
+}
+
+void PerpetualCommand::Initialize() { m_command->Initialize(); }
+
+void PerpetualCommand::Execute() { m_command->Execute(); }
+
+void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }

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

@@ -1,20 +1,16 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#pragma once
+#include "frc2/command/PrintCommand.h"
 
-#include <frc/commands/Command.h>
+using namespace frc2;
 
-class MyAutoCommand : public frc::Command {
- public:
-  MyAutoCommand();
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-  void Interrupted() override;
-};
+PrintCommand::PrintCommand(const wpi::Twine& message)
+    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+}
+
+bool PrintCommand::RunsWhenDisabled() const { return true; }

wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProxyScheduleCommand.h"
+
+using namespace frc2;
+
+ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+  SetInsert(m_toSchedule, toSchedule);
+}
+
+void ProxyScheduleCommand::Initialize() {
+  for (auto* command : m_toSchedule) {
+    command->Schedule();
+  }
+}
+
+void ProxyScheduleCommand::End(bool interrupted) {
+  if (interrupted) {
+    for (auto* command : m_toSchedule) {
+      command->Cancel();
+    }
+  }
+}
+
+void ProxyScheduleCommand::Execute() {
+  m_finished = true;
+  for (auto* command : m_toSchedule) {
+    m_finished &= !command->IsScheduled();
+  }
+}
+
+bool ProxyScheduleCommand::IsFinished() { return m_finished; }

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

@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+RunCommand::RunCommand(std::function<void()> toRun,
+                       std::initializer_list<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
+void RunCommand::Execute() { m_toRun(); }

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

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+
+ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+  SetInsert(m_toSchedule, toSchedule);
+}
+
+void ScheduleCommand::Initialize() {
+  for (auto command : m_toSchedule) {
+    command->Schedule();
+  }
+}
+
+bool ScheduleCommand::IsFinished() { return true; }
+
+bool ScheduleCommand::RunsWhenDisabled() const { return true; }

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

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+
+SequentialCommandGroup::SequentialCommandGroup(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  AddCommands(std::move(commands));
+}
+
+void SequentialCommandGroup::Initialize() {
+  m_currentCommandIndex = 0;
+
+  if (!m_commands.empty()) {
+    m_commands[0]->Initialize();
+  }
+}
+
+void SequentialCommandGroup::Execute() {
+  if (m_commands.empty()) return;
+
+  auto& currentCommand = m_commands[m_currentCommandIndex];
+
+  currentCommand->Execute();
+  if (currentCommand->IsFinished()) {
+    currentCommand->End(false);
+    m_currentCommandIndex++;
+    if (m_currentCommandIndex < m_commands.size()) {
+      m_commands[m_currentCommandIndex]->Initialize();
+    }
+  }
+}
+
+void SequentialCommandGroup::End(bool interrupted) {
+  if (interrupted && !m_commands.empty()) {
+    m_commands[m_currentCommandIndex]->End(interrupted);
+  }
+  m_currentCommandIndex = invalid_index;
+}
+
+bool SequentialCommandGroup::IsFinished() {
+  return m_currentCommandIndex == m_commands.size();
+}
+
+bool SequentialCommandGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
+
+void SequentialCommandGroup::AddCommands(
+    std::vector<std::unique_ptr<Command>>&& commands) {
+  if (!RequireUngrouped(commands)) {
+    return;
+  }
+
+  if (m_currentCommandIndex != invalid_index) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Commands cannot be added to a CommandGroup "
+                               "while the group is running");
+  }
+
+  for (auto&& command : commands) {
+    command->SetGrouped(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    m_commands.emplace_back(std::move(command));
+  }
+}

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+                                 std::function<void()> onEnd,
+                                 std::initializer_list<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+  AddRequirements(requirements);
+}
+
+StartEndCommand::StartEndCommand(const StartEndCommand& other)
+    : CommandHelper(other) {
+  m_onInit = other.m_onInit;
+  m_onEnd = other.m_onEnd;
+}
+
+void StartEndCommand::Initialize() { m_onInit(); }
+
+void StartEndCommand::End(bool interrupted) { m_onEnd(); }

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+Subsystem::~Subsystem() {
+  CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
+void Subsystem::Periodic() {}
+
+Command* Subsystem::GetDefaultCommand() const {
+  return CommandScheduler::GetInstance().GetDefaultCommand(this);
+}
+
+Command* Subsystem::GetCurrentCommand() const {
+  return CommandScheduler::GetInstance().Requiring(this);
+}
+
+void Subsystem::Register() {
+  return CommandScheduler::GetInstance().RegisterSubsystem(this);
+}

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

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SubsystemBase.h"
+
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandScheduler.h>
+
+using namespace frc2;
+
+SubsystemBase::SubsystemBase() {
+  m_name = GetTypeName(*this);
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+  builder.AddBooleanProperty(".hasDefault",
+                             [this] { return GetDefaultCommand() != nullptr; },
+                             nullptr);
+  builder.AddStringProperty(".default",
+                            [this]() -> std::string {
+                              auto command = GetDefaultCommand();
+                              if (command == nullptr) return "none";
+                              return command->GetName();
+                            },
+                            nullptr);
+  builder.AddBooleanProperty(".hasCommand",
+                             [this] { return GetCurrentCommand() != nullptr; },
+                             nullptr);
+  builder.AddStringProperty(".command",
+                            [this]() -> std::string {
+                              auto command = GetCurrentCommand();
+                              if (command == nullptr) return "none";
+                              return command->GetName();
+                            },
+                            nullptr);
+}
+
+std::string SubsystemBase::GetName() const { return m_name; }
+
+void SubsystemBase::SetName(const wpi::Twine& name) { m_name = name.str(); }
+
+std::string SubsystemBase::GetSubsystem() const { return GetName(); }
+
+void SubsystemBase::SetSubsystem(const wpi::Twine& name) { SetName(name); }
+
+void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
+  child->SetName(name);
+  frc::LiveWindow::GetInstance()->Add(child);
+}

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitCommand.h"
+
+using namespace frc2;
+
+WaitCommand::WaitCommand(double seconds) : m_duration{seconds} {
+  auto secondsStr = std::to_string(seconds);
+  SetName(wpi::Twine(m_name) + ": " + wpi::Twine(secondsStr) + " seconds");
+  m_timer = std::make_unique<frc::Timer>();
+}
+
+void WaitCommand::Initialize() {
+  m_timer->Reset();
+  m_timer->Start();
+}
+
+void WaitCommand::End(bool interrupted) { m_timer->Stop(); }
+
+bool WaitCommand::IsFinished() { return m_timer->HasPeriodPassed(m_duration); }
+
+bool WaitCommand::RunsWhenDisabled() const { return true; }

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

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitUntilCommand.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; }} {}
+
+bool WaitUntilCommand::IsFinished() { return m_condition(); }
+
+bool WaitUntilCommand::RunsWhenDisabled() const { return true; }

wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Button.h"
+
+using namespace frc2;
+
+Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
+
+Button Button::WhenPressed(Command* command, bool interruptible) {
+  WhenActive(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenPressed(std::function<void()> toRun) {
+  WhenActive(std::move(toRun));
+  return *this;
+}
+
+Button Button::WhileHeld(Command* command, bool interruptible) {
+  WhileActiveContinous(command, interruptible);
+  return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun) {
+  WhileActiveContinous(std::move(toRun));
+  return *this;
+}
+
+Button Button::WhenHeld(Command* command, bool interruptible) {
+  WhileActiveOnce(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenReleased(Command* command, bool interruptible) {
+  WhenInactive(command, interruptible);
+  return *this;
+}
+
+Button Button::WhenReleased(std::function<void()> toRun) {
+  WhenInactive(std::move(toRun));
+  return *this;
+}
+
+Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
+  ToggleWhenActive(command, interruptible);
+  return *this;
+}
+
+Button Button::CancelWhenPressed(Command* command) {
+  CancelWhenActive(command);
+  return *this;
+}

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

@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Trigger.h"
+
+#include <frc2/command/InstantCommand.h>
+
+using namespace frc2;
+
+Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
+
+Trigger Trigger::WhenActive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Schedule(interruptible);
+        }
+
+        pressedLast = pressed;
+      });
+
+  return *this;
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun) {
+  return WhenActive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (pressed) {
+          command->Schedule(interruptible);
+        } else if (pressedLast && !pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
+  return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Schedule(interruptible);
+        } else if (pressedLast && !pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (pressedLast && !pressed) {
+          command->Schedule(interruptible);
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun) {
+  return WhenInactive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command, interruptible]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          if (command->IsScheduled()) {
+            command->Cancel();
+          } else {
+            command->Schedule(interruptible);
+          }
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}
+
+Trigger Trigger::CancelWhenActive(Command* command) {
+  CommandScheduler::GetInstance().AddButton(
+      [pressedLast = Get(), *this, command]() mutable {
+        bool pressed = Get();
+
+        if (!pressedLast && pressed) {
+          command->Cancel();
+        }
+
+        pressedLast = pressed;
+      });
+  return *this;
+}

wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,8 @@ class Sendable {
   Sendable() = default;
   virtual ~Sendable() = default;
 
+  Sendable(const Sendable&) = default;
+  Sendable& operator=(const Sendable&) = default;
   Sendable(Sendable&&) = default;
   Sendable& operator=(Sendable&&) = default;
 

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

@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <frc2/command/Subsystem.h>
+
+#include <memory>
+#include <string>
+
+#include <wpi/ArrayRef.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+namespace frc2 {
+
+template <typename T>
+std::string GetTypeName(const T& type) {
+  return wpi::Demangle(typeid(type).name());
+}
+
+class ParallelCommandGroup;
+class ParallelRaceGroup;
+class ParallelDeadlineGroup;
+class SequentialCommandGroup;
+class PerpetualCommand;
+class ProxyScheduleCommand;
+
+/**
+ * A state machine representing a complete action to be performed by the robot.
+ * Commands are run by the CommandScheduler, and can be composed into
+ * CommandGroups to allow users to build complicated multi-step actions without
+ * the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading
+ * is used, unless specified explicitly from the command implementation.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * @see CommandScheduler
+ * @see CommandHelper
+ */
+class Command : public frc::ErrorBase {
+ public:
+  Command() = default;
+  Command(Command&& other) = default;
+  virtual ~Command();
+
+  /**
+   * The initial subroutine of a command.  Called once when the command is
+   * initially scheduled.
+   */
+  virtual void Initialize();
+
+  /**
+   * The main body of a command.  Called repeatedly while the command is
+   * scheduled.
+   */
+  virtual void Execute();
+
+  /**
+   * The action to take when the command ends.  Called when either the command
+   * finishes normally, or when it interrupted/canceled.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  virtual void End(bool interrupted);
+
+  /**
+   * Whether the command has finished.  Once a command finishes, the scheduler
+   * will call its end() method and un-schedule it.
+   *
+   * @return whether the command has finished.
+   */
+  virtual bool IsFinished() { return false; }
+
+  /**
+   * Specifies the set of subsystems used by this command.  Two commands cannot
+   * use the same subsystem at the same time.  If the command is scheduled as
+   * interruptible and another command is scheduled that shares a requirement,
+   * the command will be interrupted.  Else, the command will not be scheduled.
+   * If no subsystems are required, return an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the
+   * requirements as a field, and return that field here, rather than allocating
+   * a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   */
+  virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
+
+  /**
+   * Decorates this command with a timeout.  If the specified timeout is
+   * exceeded before the command finishes normally, the command will be
+   * interrupted and un-scheduled.  Note that the timeout only applies to the
+   * command returned by this method; the calling command is not itself changed.
+   *
+   * @param seconds the timeout duration
+   * @return the command with the timeout added
+   */
+  ParallelRaceGroup WithTimeout(double seconds) &&;
+
+  /**
+   * Decorates this command with an interrupt condition.  If the specified
+   * condition becomes true before the command finishes normally, the command
+   * will be interrupted and un-scheduled. Note that this only applies to the
+   * command returned by this method; the calling command is not itself changed.
+   *
+   * @param condition the interrupt condition
+   * @return the command with the interrupt condition added
+   */
+  ParallelRaceGroup InterruptOn(std::function<bool()> condition) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @return the decorated command
+   */
+  SequentialCommandGroup WhenFinished(std::function<void()> toRun) &&;
+
+  /**
+   * Decorates this command to run perpetually, ignoring its ordinary end
+   * conditions.  The decorated command can still be interrupted or canceled.
+   *
+   * @return the decorated command
+   */
+  PerpetualCommand Perpetually() &&;
+
+  /**
+   * Decorates this command to run "by proxy" by wrapping it in a {@link
+   * ProxyScheduleCommand}. This is useful for "forking off" from command groups
+   * when the user does not wish to extend the command's requirements to the
+   * entire command group.
+   *
+   * @return the decorated command
+   */
+  ProxyScheduleCommand AsProxy();
+
+  /**
+   * Schedules this command.
+   *
+   * @param interruptible whether this command can be interrupted by another
+   * command that shares one of its requirements
+   */
+  void Schedule(bool interruptible);
+
+  /**
+   * Schedules this command, defaulting to interruptible.
+   */
+  void Schedule() { Schedule(true); }
+
+  /**
+   * Cancels this command.  Will call the command's interrupted() method.
+   * Commands will be canceled even if they are not marked as interruptible.
+   */
+  void Cancel();
+
+  /**
+   * Whether or not the command is currently scheduled.  Note that this does not
+   * detect whether the command is being run by a CommandGroup, only whether it
+   * is directly being run by the scheduler.
+   *
+   * @return Whether the command is scheduled.
+   */
+  bool IsScheduled() const;
+
+  /**
+   * Whether the command requires a given subsystem.  Named "hasRequirement"
+   * rather than "requires" to avoid confusion with
+   * {@link
+   * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
+   *  - this may be able to be changed in a few years.
+   *
+   * @param requirement the subsystem to inquire about
+   * @return whether the subsystem is required
+   */
+  bool HasRequirement(Subsystem* requirement) const;
+
+  /**
+   * Whether the command is currently grouped in a command group.  Used as extra
+   * insurance to prevent accidental independent use of grouped commands.
+   */
+  bool IsGrouped() const;
+
+  /**
+   * Sets whether the command is currently grouped in a command group.  Can be
+   * used to "reclaim" a command if a group is no longer going to use it.  NOT
+   * ADVISED!
+   */
+  void SetGrouped(bool grouped);
+
+  /**
+   * Whether the given command should run when the robot is disabled.  Override
+   * to return true if the command should run when disabled.
+   *
+   * @return whether the command should run when the robot is disabled
+   */
+  virtual bool RunsWhenDisabled() const { return false; }
+
+  virtual std::string GetName() const;
+
+ protected:
+  /**
+   * Transfers ownership of this command to a unique pointer.  Used for
+   * decorator methods.
+   */
+  virtual std::unique_ptr<Command> TransferOwnership() && = 0;
+
+  bool m_isGrouped = false;
+};
+
+/**
+ * Checks if two commands have disjoint requirement sets.
+ *
+ * @param first The first command to check.
+ * @param second The second command to check.
+ * @return False if first and second share a requirement.
+ */
+bool RequirementsDisjoint(Command* first, Command* second);
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/CommandBase.h

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+
+#include <string>
+
+#include <wpi/SmallSet.h>
+#include <wpi/SmallVector.h>
+
+#include "Command.h"
+
+namespace frc2 {
+/**
+ * A Sendable base class for Commands.
+ */
+class CommandBase : public frc::Sendable, public Command {
+ public:
+  CommandBase(CommandBase&& other) = default;
+
+  CommandBase(const CommandBase& other);
+
+  /**
+   * Adds the specified requirements to the command.
+   *
+   * @param requirements the requirements to add
+   */
+  void AddRequirements(std::initializer_list<Subsystem*> requirements);
+
+  void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
+
+  wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
+
+  void SetName(const wpi::Twine& name) override;
+
+  std::string GetName() const override;
+
+  std::string GetSubsystem() const override;
+
+  void SetSubsystem(const wpi::Twine& subsystem) override;
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+  CommandBase();
+  std::string m_name;
+  std::string m_subsystem;
+  wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "CommandBase.h"
+
+namespace frc2 {
+
+/**
+ * A base for CommandGroups.  Statically tracks commands that have been
+ * allocated to groups to ensure those commands are not also used independently,
+ * which can result in inconsistent command state and unpredictable execution.
+ */
+class CommandGroupBase : public CommandBase {
+ public:
+  /**
+   * Requires that the specified command not have been already allocated to a
+   * CommandGroup. Reports an error if the command is already grouped.
+   *
+   * @param commands The command to check
+   * @return True if all the command is ungrouped.
+   */
+  static bool RequireUngrouped(Command& command);
+
+  /**
+   * Requires that the specified commands not have been already allocated to a
+   * CommandGroup. Reports an error if any of the commands are already grouped.
+   *
+   * @param commands The commands to check
+   * @return True if all the commands are ungrouped.
+   */
+  static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
+
+  /**
+   * Requires that the specified commands not have been already allocated to a
+   * CommandGroup. Reports an error if any of the commands are already grouped.
+   *
+   * @param commands The commands to check
+   * @return True if all the commands are ungrouped.
+   */
+  static bool RequireUngrouped(std::initializer_list<Command*>);
+
+  /**
+   * Adds the given commands to the command group.
+   *
+   * @param commands The commands to add.
+   */
+  virtual void AddCommands(
+      std::vector<std::unique_ptr<Command>>&& commands) = 0;
+};
+}  // namespace frc2

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "Command.h"
+
+namespace frc2 {
+
+/**
+ * CRTP implementation to allow polymorphic decorator functions in Command.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ */
+template <typename Base, typename CRTP,
+          typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
+class CommandHelper : public Base {
+  using Base::Base;
+
+ public:
+  CommandHelper() = default;
+
+ protected:
+  std::unique_ptr<Command> TransferOwnership() && override {
+    return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
+  }
+};
+}  // namespace frc2

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

@@ -0,0 +1,362 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/SendableBase.h>
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+#include <wpi/FunctionExtras.h>
+#include <wpi/SmallSet.h>
+
+#include "CommandState.h"
+
+namespace frc2 {
+class Command;
+class Subsystem;
+
+/**
+ * The scheduler responsible for running Commands.  A Command-based robot should
+ * call Run() on the singleton instance in its periodic block in order to run
+ * commands synchronously from the main loop.  Subsystems should be registered
+ * with the scheduler using RegisterSubsystem() in order for their Periodic()
+ * methods to be called and for their default commands to be scheduled.
+ */
+class CommandScheduler final : public frc::SendableBase, frc::ErrorBase {
+ public:
+  /**
+   * Returns the Scheduler instance.
+   *
+   * @return the instance
+   */
+  static CommandScheduler& GetInstance();
+
+  using Action = std::function<void(const Command&)>;
+
+  /**
+   * Adds a button binding to the scheduler, which will be polled to schedule
+   * commands.
+   *
+   * @param button The button to add
+   */
+  void AddButton(wpi::unique_function<void()> button);
+
+  /**
+   * Removes all button bindings from the scheduler.
+   */
+  void ClearButtons();
+
+  /**
+   * Schedules a command for execution.  Does nothing if the command is already
+   * scheduled. If a command's requirements are not available, it will only be
+   * started if all the commands currently using those requirements have been
+   * scheduled as interruptible.  If this is the case, they will be interrupted
+   * and the command will be scheduled.
+   *
+   * @param interruptible whether this command can be interrupted
+   * @param command       the command to schedule
+   */
+  void Schedule(bool interruptible, Command* command);
+
+  /**
+   * Schedules a command for execution, with interruptible defaulted to true.
+   * Does nothing if the command is already scheduled.
+   *
+   * @param command the command to schedule
+   */
+  void Schedule(Command* command);
+
+  /**
+   * Schedules multiple commands for execution.  Does nothing if the command is
+   * already scheduled. If a command's requirements are not available, it will
+   * only be started if all the commands currently using those requirements have
+   * been scheduled as interruptible.  If this is the case, they will be
+   * interrupted and the command will be scheduled.
+   *
+   * @param interruptible whether the commands should be interruptible
+   * @param commands      the commands to schedule
+   */
+  void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
+
+  /**
+   * Schedules multiple commands for execution.  Does nothing if the command is
+   * already scheduled. If a command's requirements are not available, it will
+   * only be started if all the commands currently using those requirements have
+   * been scheduled as interruptible.  If this is the case, they will be
+   * interrupted and the command will be scheduled.
+   *
+   * @param interruptible whether the commands should be interruptible
+   * @param commands      the commands to schedule
+   */
+  void Schedule(bool interruptible, std::initializer_list<Command*> commands);
+
+  /**
+   * Schedules multiple commands for execution, with interruptible defaulted to
+   * true.  Does nothing if the command is already scheduled.
+   *
+   * @param commands the commands to schedule
+   */
+  void Schedule(wpi::ArrayRef<Command*> commands);
+
+  /**
+   * Schedules multiple commands for execution, with interruptible defaulted to
+   * true.  Does nothing if the command is already scheduled.
+   *
+   * @param commands the commands to schedule
+   */
+  void Schedule(std::initializer_list<Command*> commands);
+
+  /**
+   * Runs a single iteration of the scheduler.  The execution occurs in the
+   * following order:
+   *
+   * <p>Subsystem periodic methods are called.
+   *
+   * <p>Button bindings are polled, and new commands are scheduled from them.
+   *
+   * <p>Currently-scheduled commands are executed.
+   *
+   * <p>End conditions are checked on currently-scheduled commands, and commands
+   * that are finished have their end methods called and are removed.
+   *
+   * <p>Any subsystems not being used as requirements have their default methods
+   * started.
+   */
+  void Run();
+
+  /**
+   * Registers subsystems with the scheduler.  This must be called for the
+   * subsystem's periodic block to run when the scheduler is run, and for the
+   * subsystem's default command to be scheduled.  It is recommended to call
+   * this from the constructor of your subsystem implementations.
+   *
+   * @param subsystem the subsystem to register
+   */
+  void RegisterSubsystem(Subsystem* subsystem);
+
+  /**
+   * Un-registers subsystems with the scheduler.  The subsystem will no longer
+   * have its periodic block called, and will not have its default command
+   * scheduled.
+   *
+   * @param subsystem the subsystem to un-register
+   */
+  void UnregisterSubsystem(Subsystem* subsystem);
+
+  void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+  void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+  /**
+   * Sets the default command for a subsystem.  Registers that subsystem if it
+   * is not already registered.  Default commands will run whenever there is no
+   * other command currently scheduled that requires the subsystem.  Default
+   * commands should be written to never end (i.e. their IsFinished() method
+   * should return false), as they would simply be re-scheduled if they do.
+   * Default commands must also require their subsystem.
+   *
+   * @param subsystem      the subsystem whose default command will be set
+   * @param defaultCommand the default command to associate with the subsystem
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
+    if (!defaultCommand.HasRequirement(subsystem)) {
+      wpi_setWPIErrorWithContext(
+          CommandIllegalUse, "Default commands must require their subsystem!");
+      return;
+    }
+    if (defaultCommand.IsFinished()) {
+      wpi_setWPIErrorWithContext(CommandIllegalUse,
+                                 "Default commands should not end!");
+      return;
+    }
+    m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
+        std::forward<T>(defaultCommand));
+  }
+
+  /**
+   * Gets the default command associated with this subsystem.  Null if this
+   * subsystem has no default command associated with it.
+   *
+   * @param subsystem the subsystem to inquire about
+   * @return the default command associated with the subsystem
+   */
+  Command* GetDefaultCommand(const Subsystem* subsystem) const;
+
+  /**
+   * Cancels a command.  The scheduler will only call the interrupted method of
+   * a canceled command, not the end method (though the interrupted method may
+   * itself call the end method).  Commands will be canceled even if they are
+   * not scheduled as interruptible.
+   *
+   * @param command the command to cancel
+   */
+  void Cancel(Command* command);
+
+  /**
+   * Cancels commands.  The scheduler will only call the interrupted method of a
+   * canceled command, not the end method (though the interrupted method may
+   * itself call the end method).  Commands will be canceled even if they are
+   * not scheduled as interruptible.
+   *
+   * @param commands the commands to cancel
+   */
+  void Cancel(wpi::ArrayRef<Command*> commands);
+
+  /**
+   * Cancels commands.  The scheduler will only call the interrupted method of a
+   * canceled command, not the end method (though the interrupted method may
+   * itself call the end method).  Commands will be canceled even if they are
+   * not scheduled as interruptible.
+   *
+   * @param commands the commands to cancel
+   */
+  void Cancel(std::initializer_list<Command*> commands);
+
+  /**
+   * Cancels all commands that are currently scheduled.
+   */
+  void CancelAll();
+
+  /**
+   * Returns the time since a given command was scheduled.  Note that this only
+   * works on commands that are directly scheduled by the scheduler; it will not
+   * work on commands inside of commandgroups, as the scheduler does not see
+   * them.
+   *
+   * @param command the command to query
+   * @return the time since the command was scheduled, in seconds
+   */
+  double TimeSinceScheduled(const Command* command) const;
+
+  /**
+   * Whether the given commands are running.  Note that this only works on
+   * commands that are directly scheduled by the scheduler; it will not work on
+   * commands inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
+
+  /**
+   * Whether the given commands are running.  Note that this only works on
+   * commands that are directly scheduled by the scheduler; it will not work on
+   * commands inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(std::initializer_list<const Command*> commands) const;
+
+  /**
+   * Whether a given command is running.  Note that this only works on commands
+   * that are directly scheduled by the scheduler; it will not work on commands
+   * inside of CommandGroups, as the scheduler does not see them.
+   *
+   * @param commands the command to query
+   * @return whether the command is currently scheduled
+   */
+  bool IsScheduled(const Command* command) const;
+
+  /**
+   * Returns the command currently requiring a given subsystem.  Null if no
+   * command is currently requiring the subsystem
+   *
+   * @param subsystem the subsystem to be inquired about
+   * @return the command currently requiring the subsystem
+   */
+  Command* Requiring(const Subsystem* subsystem) const;
+
+  /**
+   * Disables the command scheduler.
+   */
+  void Disable();
+
+  /**
+   * Enables the command scheduler.
+   */
+  void Enable();
+
+  /**
+   * Adds an action to perform on the initialization of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInitialize(Action action);
+
+  /**
+   * Adds an action to perform on the execution of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandExecute(Action action);
+
+  /**
+   * Adds an action to perform on the interruption of any command by the
+   * scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandInterrupt(Action action);
+
+  /**
+   * Adds an action to perform on the finishing of any command by the scheduler.
+   *
+   * @param action the action to perform
+   */
+  void OnCommandFinish(Action action);
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+  // 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;
+
+  // 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;
+
+  friend class CommandTestBase;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/CommandState.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc2 {
+/**
+ * Class that holds scheduling state for a command.  Used internally by the
+ * CommandScheduler
+ */
+class CommandState final {
+ public:
+  CommandState() = default;
+
+  explicit CommandState(bool interruptible);
+
+  bool IsInterruptible() const { return m_interruptible; }
+
+  // The time since this command was initialized.
+  double TimeSinceInitialized() const;
+
+ private:
+  double m_startTime = -1;
+  bool m_interruptible;
+
+  void StartTiming();
+  void StartRunning();
+};
+}  // namespace frc2

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

@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * Runs one of two commands, depending on the value of the given condition when
+ * this command is initialized.  Does not actually schedule the selected command
+ * - rather, the command is run through this command; this ensures that the
+ * command will behave as expected if used as part of a CommandGroup.  Requires
+ * the requirements of both commands, again to ensure proper functioning when
+ * used in a CommandGroup.  If this is undesired, consider using
+ * ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ *
+ * @see ScheduleCommand
+ */
+class ConditionalCommand
+    : public CommandHelper<CommandBase, ConditionalCommand> {
+ public:
+  /**
+   * Creates a new ConditionalCommand.
+   *
+   * @param onTrue    the command to run if the condition is true
+   * @param onFalse   the command to run if the condition is false
+   * @param condition the condition to determine which command to run
+   */
+  template <class T1, class T2,
+            typename = std::enable_if_t<
+                std::is_base_of<Command, std::remove_reference_t<T1>>::value>,
+            typename = std::enable_if_t<
+                std::is_base_of<Command, std::remove_reference_t<T2>>::value>>
+  ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
+      : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
+                               std::forward<T1>(onTrue)),
+                           std::make_unique<std::remove_reference_t<T2>>(
+                               std::forward<T2>(onFalse)),
+                           condition) {}
+
+  /**
+   * Creates a new ConditionalCommand.
+   *
+   * @param onTrue    the command to run if the condition is true
+   * @param onFalse   the command to run if the condition is false
+   * @param condition the condition to determine which command to run
+   */
+  ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+                     std::unique_ptr<Command>&& onFalse,
+                     std::function<bool()> condition);
+
+  ConditionalCommand(ConditionalCommand&& other) = default;
+
+  // No copy constructors for command groups
+  ConditionalCommand(const ConditionalCommand& other) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  std::unique_ptr<Command> m_onTrue;
+  std::unique_ptr<Command> m_onFalse;
+  std::function<bool()> m_condition;
+  Command* m_selectedCommand{nullptr};
+  bool m_runsWhenDisabled = true;
+};
+}  // namespace frc2

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

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that allows the user to pass in functions for each of the basic
+ * command methods through the constructor.  Useful for inline definitions of
+ * complex commands - note, however, that if a command is beyond a certain
+ * complexity it is usually better practice to write a proper class for it than
+ * to inline it.
+ */
+class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
+ public:
+  /**
+   * Creates a new FunctionalCommand.
+   *
+   * @param onInit       the function to run on command initialization
+   * @param onExecute    the function to run on command execution
+   * @param onEnd        the function to run on command end
+   * @param isFinished   the function that determines whether the command has
+   * finished
+   * @param requirements the subsystems required by this command
+   */
+  FunctionalCommand(std::function<void()> onInit,
+                    std::function<void()> onExecute,
+                    std::function<void(bool)> onEnd,
+                    std::function<bool()> isFinished);
+
+  FunctionalCommand(FunctionalCommand&& other) = default;
+
+  FunctionalCommand(const FunctionalCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  std::function<void()> m_onInit;
+  std::function<void()> m_onExecute;
+  std::function<void(bool)> m_onEnd;
+  std::function<bool()> m_isFinished;
+};
+}  // namespace frc2

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on
+ * the same iteration of the scheduler.  Users can either pass in a Runnable and
+ * a set of requirements, or else subclass this command if desired.
+ */
+class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
+ public:
+  /**
+   * Creates a new InstantCommand that runs the given Runnable with the given
+   * requirements.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems required by this command
+   */
+  InstantCommand(std::function<void()> toRun,
+                 std::initializer_list<Subsystem*> requirements);
+
+  InstantCommand(InstantCommand&& other) = default;
+
+  InstantCommand(const InstantCommand& other) = default;
+
+  /**
+   * Creates a new InstantCommand with a Runnable that does nothing.  Useful
+   * only as a no-arg constructor to call implicitly from subclass constructors.
+   */
+  InstantCommand();
+
+  void Initialize() override;
+
+  bool IsFinished() final;
+
+ private:
+  std::function<void()> m_toRun;
+};
+}  // namespace frc2

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

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Notifier.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that starts a notifier to run the given runnable periodically in a
+ * separate thread. Has no end condition as-is; either subclass it or use {@link
+ * Command#withTimeout(double)} or
+ * {@link Command#interruptOn(BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to
+ * make the executed code thread-safe.  If you do not know what "thread-safe"
+ * means, that is a good sign that you should not use this class.
+ */
+class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
+ public:
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun        the runnable for the notifier to run
+   * @param period       the period at which the notifier should run, in seconds
+   * @param requirements the subsystems required by this command
+   */
+  NotifierCommand(std::function<void()> toRun, double period,
+                  std::initializer_list<Subsystem*> requirements);
+
+  NotifierCommand(NotifierCommand&& other);
+
+  NotifierCommand(const NotifierCommand& other);
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+ private:
+  std::function<void()> m_toRun;
+  frc::Notifier m_notifier;
+  double m_period;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/PIDCommand.h

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a PIDController.  Runs forever by
+ * default - to add exit conditions and/or other behavior, subclass this class.
+ * The controller calculation and output are performed synchronously in the
+ * command's execute() method.
+ *
+ * @see PIDController
+ */
+class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
+ public:
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * PIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpointSource   the controller's reference (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource,
+             std::function<double()> setpointSource,
+             std::function<void(double)> useOutput,
+             std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * PIDController with a constant setpoint.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpoint         the controller's setpoint (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource, double setpoint,
+             std::function<void(double)> useOutput,
+             std::initializer_list<Subsystem*> requirements);
+
+  PIDCommand(PIDCommand&& other) = default;
+
+  PIDCommand(const PIDCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  /**
+   * Sets the function that uses the output of the PIDController.
+   *
+   * @param useOutput The function that uses the output.
+   */
+  void SetOutput(std::function<void(double)> useOutput);
+
+  /**
+   * Sets the setpoint for the controller to track the given source.
+   *
+   * @param setpointSource The setpoint source
+   */
+  void SetSetpoint(std::function<double()> setpointSource);
+
+  /**
+   * Sets the setpoint for the controller to a constant value.
+   *
+   * @param setpoint The setpoint
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Sets the setpoint for the controller to a constant value relative (i.e.
+   * added to) the current setpoint.
+   *
+   * @param relativeReference The change in setpoint
+   */
+  void SetSetpointRelative(double relativeSetpoint);
+
+  /**
+   * Returns the PIDController used by the command.
+   *
+   * @return The PIDController
+   */
+  PIDController& getController();
+
+ protected:
+  PIDController m_controller;
+  std::function<double()> m_measurement;
+  std::function<double()> m_setpoint;
+  std::function<void(double)> m_useOutput;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a PIDController to control an output.  The controller
+ * is run synchronously from the subsystem's periodic() method.
+ *
+ * @see PIDController
+ */
+class PIDSubsystem : public SubsystemBase {
+ public:
+  /**
+   * Creates a new PIDSubsystem.
+   *
+   * @param controller the PIDController to use
+   */
+  explicit PIDSubsystem(PIDController controller);
+
+  void Periodic() override;
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   */
+  virtual void UseOutput(double output) = 0;
+
+  /**
+   * Returns the reference (setpoint) used by the PIDController.
+   *
+   * @return the reference (setpoint) to be used by the controller
+   */
+  virtual double GetSetpoint() = 0;
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual double GetMeasurement() = 0;
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  virtual void Enable();
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable();
+
+  /**
+   * Returns the PIDController.
+   *
+   * @return The controller.
+   */
+  PIDController& GetController();
+
+ protected:
+  PIDController m_controller;
+  bool m_enabled;
+};
+}  // namespace frc2

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

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when the last
+ * command ends.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelCommandGroup
+    : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
+ public:
+  /**
+   * Creates a new ParallelCommandGroup.  The given commands will be executed
+   * simultaneously. The command group will finish when the last command
+   * finishes.  If the CommandGroup is interrupted, only the commands that are
+   * still running will be interrupted.
+   *
+   * @param commands the commands to include in this group.
+   */
+  explicit ParallelCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * Creates a new ParallelCommandGroup.  The given commands will be executed
+   * simultaneously. The command group will finish when the last command
+   * finishes.  If the CommandGroup is interrupted, only the commands that are
+   * still running will be interrupted.
+   *
+   * @param commands the commands to include in this group.
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelCommandGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelCommandGroup(ParallelCommandGroup&& other) = default;
+
+  // No copy constructors for commandgroups
+  ParallelCommandGroup(const ParallelCommandGroup&) = delete;
+
+  template <class... Types>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  bool m_runWhenDisabled{true};
+  bool isRunning = false;
+};
+}  // namespace frc2

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

@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending only when a
+ * specific command (the "deadline") ends, interrupting all other commands that
+ * are still running at that point.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelDeadlineGroup
+    : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
+ public:
+  /**
+   * Creates a new ParallelDeadlineGroup.  The given commands (including the
+   * deadline) will be executed simultaneously.  The CommandGroup will finish
+   * when the deadline finishes, interrupting all other still-running commands.
+   * If the CommandGroup is interrupted, only the commands still running will be
+   * interrupted.
+   *
+   * @param deadline the command that determines when the group ends
+   * @param commands the commands to be executed
+   */
+  ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
+                        std::vector<std::unique_ptr<Command>>&& commands);
+  /**
+   * Creates a new ParallelDeadlineGroup.  The given commands (including the
+   * deadline) will be executed simultaneously.  The CommandGroup will finish
+   * when the deadline finishes, interrupting all other still-running commands.
+   * If the CommandGroup is interrupted, only the commands still running will be
+   * interrupted.
+   *
+   * @param deadline the command that determines when the group ends
+   * @param commands the commands to be executed
+   */
+  template <class T, class... Types,
+            typename = std::enable_if_t<
+                std::is_base_of<Command, std::remove_reference_t<T>>::value>,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
+    SetDeadline(std::make_unique<std::remove_reference_t<T>>(
+        std::forward<T>(deadline)));
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  void SetDeadline(std::unique_ptr<Command>&& deadline);
+
+  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  Command* m_deadline;
+  bool m_runWhenDisabled{true};
+  bool isRunning = false;
+};
+}  // namespace frc2

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

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <set>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when any one
+ * of the commands ends and interrupting all the others.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelRaceGroup
+    : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
+ public:
+  /**
+   * Creates a new ParallelCommandRace.  The given commands will be executed
+   * simultaneously, and will "race to the finish" - the first command to finish
+   * ends the entire command, with all other commands being interrupted.
+   *
+   * @param commands the commands to include in this group.
+   */
+  explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit ParallelRaceGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  ParallelRaceGroup(ParallelRaceGroup&& other) = default;
+
+  // No copy constructors for command groups
+  ParallelRaceGroup(const ParallelRaceGroup&) = delete;
+
+  template <class... Types>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+  std::set<std::unique_ptr<Command>> m_commands;
+  bool m_runWhenDisabled{true};
+  bool m_finished{false};
+  bool isRunning = false;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs another command in perpetuity, ignoring that command's
+ * end conditions.  While this class does not extend {@link CommandGroupBase},
+ * it is still considered a CommandGroup, as it allows one to compose another
+ * command within it; the command instances that are passed to it cannot be
+ * added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
+ public:
+  /**
+   * Creates a new PerpetualCommand.  Will run another command in perpetuity,
+   * ignoring that command's end conditions, unless this command itself is
+   * interrupted.
+   *
+   * @param command the command to run perpetually
+   */
+  explicit PerpetualCommand(std::unique_ptr<Command>&& command);
+
+  /**
+   * Creates a new PerpetualCommand.  Will run another command in perpetuity,
+   * ignoring that command's end conditions, unless this command itself is
+   * interrupted.
+   *
+   * @param command the command to run perpetually
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  explicit PerpetualCommand(T&& command)
+      : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
+            std::forward<T>(command))) {}
+
+  PerpetualCommand(PerpetualCommand&& other) = default;
+
+  // No copy constructors for command groups
+  PerpetualCommand(const PerpetualCommand& other) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+ private:
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2

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

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+#include "CommandHelper.h"
+#include "InstantCommand.h"
+
+namespace frc2 {
+/**
+ * A command that prints a string when initialized.
+ */
+class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
+ public:
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  explicit PrintCommand(const wpi::Twine& message);
+
+  PrintCommand(PrintCommand&& other) = default;
+
+  PrintCommand(const PrintCommand& other) = default;
+
+  bool RunsWhenDisabled() const override;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized, and ends when
+ * all the commands are no longer scheduled.  Useful for forking off from
+ * CommandGroups.  If this command is interrupted, it will cancel all of the
+ * commands.
+ */
+class ProxyScheduleCommand
+    : public CommandHelper<CommandBase, ProxyScheduleCommand> {
+ public:
+  /**
+   * Creates a new ProxyScheduleCommand that schedules the given commands when
+   * initialized, and ends when they are all no longer scheduled.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+  ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
+
+  ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  void Execute() override;
+
+  bool IsFinished() override;
+
+ private:
+  wpi::SmallVector<Command*, 4> m_toSchedule;
+  bool m_finished{false};
+};
+}  // namespace frc2

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a Runnable continuously.  Has no end condition as-is;
+ * either subclass it or use Command.WithTimeout() or
+ * Command.InterruptOn() to give it one.  If you only wish
+ * to execute a Runnable once, use InstantCommand.
+ */
+class RunCommand : public CommandHelper<CommandBase, RunCommand> {
+ public:
+  /**
+   * Creates a new RunCommand.  The Runnable will be run continuously until the
+   * command ends.  Does not run when disabled.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems to require
+   */
+  RunCommand(std::function<void()> toRun,
+             std::initializer_list<Subsystem*> requirements);
+
+  RunCommand(RunCommand&& other) = default;
+
+  RunCommand(const RunCommand& other) = default;
+
+  void Execute();
+
+ protected:
+  std::function<void()> m_toRun;
+};
+}  // namespace frc2

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

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized.  Useful for
+ * forking off from CommandGroups.  Note that if run from a CommandGroup, the
+ * group will not know about the status of the scheduled commands, and will
+ * treat this command as finishing instantly.
+ */
+class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
+ public:
+  /**
+   * Creates a new ScheduleCommand that schedules the given commands when
+   * initialized.
+   *
+   * @param toSchedule the commands to schedule
+   */
+  explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+  ScheduleCommand(ScheduleCommand&& other) = default;
+
+  ScheduleCommand(const ScheduleCommand& other) = default;
+
+  void Initialize() override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  wpi::SmallVector<Command*, 4> m_toSchedule;
+};
+}  // namespace frc2

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

@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "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.
+ * Does not actually schedule the selected command - rather, the command is run
+ * through this command; this ensures that the command will behave as expected
+ * if used as part of a CommandGroup.  Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup.  If
+ * this is undesired, consider using ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
+ public:
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param commands the map of commands to choose from
+   * @param selector the selector to determine which command to run
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  SelectCommand(std::function<Key()> selector,
+                std::pair<Key, Types>... commands)
+      : m_selector{std::move(selector)} {
+    std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
+
+    ((void)foo.emplace_back(commands.first,
+                            std::make_unique<std::remove_reference_t<Types>>(
+                                std::move(commands.second))),
+     ...);
+
+    for (auto&& command : foo) {
+      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+        return;
+      }
+    }
+
+    for (auto&& command : foo) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  SelectCommand(
+      std::function<Key()> selector,
+      std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
+      : m_selector{std::move(selector)} {
+    for (auto&& command : commands) {
+      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+        return;
+      }
+    }
+
+    for (auto&& command : commands) {
+      this->AddRequirements(command.second->GetRequirements());
+      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+      m_commands.emplace(std::move(command.first), std::move(command.second));
+    }
+  }
+
+  // No copy constructors for command groups
+  SelectCommand(const SelectCommand& other) = delete;
+
+  /**
+   * Creates a new selectcommand.
+   *
+   * @param toRun a supplier providing the command to run
+   */
+  explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
+
+  SelectCommand(SelectCommand&& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override { m_selectedCommand->Execute(); }
+
+  void End(bool interrupted) override {
+    return m_selectedCommand->End(interrupted);
+  }
+
+  bool IsFinished() override { return m_selectedCommand->IsFinished(); }
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ protected:
+  std::unique_ptr<Command> TransferOwnership() && override {
+    return std::make_unique<SelectCommand>(std::move(*this));
+  }
+
+ private:
+  std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
+  std::function<Key()> m_selector;
+  std::function<Command*()> m_toRun;
+  Command* m_selectedCommand;
+  bool m_runsWhenDisabled = true;
+};
+
+template <typename T>
+void SelectCommand<T>::Initialize() {
+  if (m_selector) {
+    auto find = m_commands.find(m_selector());
+    if (find == m_commands.end()) {
+      m_selectedCommand = new PrintCommand(
+          "SelectCommand selector value does not correspond to any command!");
+      return;
+    }
+    m_selectedCommand = find->second.get();
+  } else {
+    m_selectedCommand = m_toRun();
+  }
+  m_selectedCommand->Initialize();
+}
+
+}  // namespace frc2

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

@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+#include "frc/ErrorBase.h"
+#include "frc/WPIErrors.h"
+
+namespace frc2 {
+
+const size_t invalid_index = std::numeric_limits<size_t>::max();
+
+/**
+ * A CommandGroups that runs a list of commands in sequence.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class SequentialCommandGroup
+    : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
+ public:
+  /**
+   * Creates a new SequentialCommandGroup.  The given commands will be run
+   * sequentially, with the CommandGroup finishing when the last command
+   * finishes.
+   *
+   * @param commands the commands to include in this group.
+   */
+  explicit SequentialCommandGroup(
+      std::vector<std::unique_ptr<Command>>&& commands);
+
+  /**
+   * Creates a new SequentialCommandGroup.  The given commands will be run
+   * sequentially, with the CommandGroup finishing when the last command
+   * finishes.
+   *
+   * @param commands the commands to include in this group.
+   */
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  explicit SequentialCommandGroup(Types&&... commands) {
+    AddCommands(std::forward<Types>(commands)...);
+  }
+
+  SequentialCommandGroup(SequentialCommandGroup&& other) = default;
+
+  // No copy constructors for command groups
+  SequentialCommandGroup(const SequentialCommandGroup&) = delete;
+
+  template <class... Types,
+            typename = std::enable_if_t<std::conjunction_v<
+                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+  void AddCommands(Types&&... commands) {
+    std::vector<std::unique_ptr<Command>> foo;
+    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+         std::forward<Types>(commands))),
+     ...);
+    AddCommands(std::move(foo));
+  }
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
+
+  wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
+  size_t m_currentCommandIndex{invalid_index};
+  bool m_runWhenDisabled{true};
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/SetUtilities.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+namespace frc2 {
+template <typename T>
+void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
+  for (auto addCommand : toAdd) {
+    bool exists = false;
+    for (auto existingCommand : vector) {
+      if (addCommand == existingCommand) {
+        exists = true;
+        break;
+      }
+    }
+    if (!exists) {
+      vector.emplace_back(addCommand);
+    }
+  }
+}
+}  // namespace frc2

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

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a given runnable when it is initalized, and another
+ * runnable when it ends. Useful for running and then stopping a motor, or
+ * extending and then retracting a solenoid. Has no end condition as-is; either
+ * subclass it or use Command.WithTimeout() or Command.InterruptOn() to give it
+ * one.
+ */
+class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
+ public:
+  /**
+   * Creates a new StartEndCommand.  Will run the given runnables when the
+   * command starts and when it ends.
+   *
+   * @param onInit       the Runnable to run on command init
+   * @param onEnd        the Runnable to run on command end
+   * @param requirements the subsystems required by this command
+   */
+  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+                  std::initializer_list<Subsystem*> requirements);
+
+  StartEndCommand(StartEndCommand&& other) = default;
+
+  StartEndCommand(const StartEndCommand& other);
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+ protected:
+  std::function<void()> m_onInit;
+  std::function<void()> m_onEnd;
+};
+}  // namespace frc2

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

@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandScheduler.h>
+
+#include <utility>
+
+namespace frc2 {
+class Command;
+/**
+ * A robot subsystem.  Subsystems are the basic unit of robot organization in
+ * the Command-based framework; they encapsulate low-level hardware objects
+ * (motor controllers, sensors, etc) and provide methods through which they can
+ * be used by Commands.  Subsystems are used by the CommandScheduler's resource
+ * management system to ensure multiple robot actions are not "fighting" over
+ * the same hardware; Commands that use a subsystem should include that
+ * subsystem in their GetRequirements() method, and resources used within a
+ * subsystem should generally remain encapsulated and not be shared by other
+ * parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the
+ * CommandScheduler.RegisterSubsystem() method in order for the
+ * Periodic() method to be called.  It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations.  The
+ * SubsystemBase class offers a simple base for user implementations
+ * that handles this.
+ *
+ * @see Command
+ * @see CommandScheduler
+ * @see SubsystemBase
+ */
+class Subsystem {
+ public:
+  ~Subsystem();
+  /**
+   * This method is called periodically by the CommandScheduler.  Useful for
+   * updating subsystem-specific state that you don't want to offload to a
+   * Command.  Teams should try to be consistent within their own codebases
+   * about which responsibilities will be handled by Commands, and which will be
+   * handled here.
+   */
+  virtual void Periodic();
+
+  /**
+   * Sets the default Command of the subsystem.  The default command will be
+   * automatically scheduled when no other commands are scheduled that require
+   * the subsystem. Default commands should generally not end on their own, i.e.
+   * their IsFinished() method should always return false.  Will automatically
+   * register this subsystem with the CommandScheduler.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  void SetDefaultCommand(T&& defaultCommand) {
+    CommandScheduler::GetInstance().SetDefaultCommand(
+        this, std::forward<T>(defaultCommand));
+  }
+
+  /**
+   * Gets the default command for this subsystem.  Returns null if no default
+   * command is currently associated with the subsystem.
+   *
+   * @return the default command associated with this subsystem
+   */
+  Command* GetDefaultCommand() const;
+
+  /**
+   * Returns the command currently running on this subsystem.  Returns null if
+   * no command is currently scheduled that requires this subsystem.
+   *
+   * @return the scheduled command currently requiring this subsystem
+   */
+  Command* GetCurrentCommand() const;
+
+  /**
+   * Registers this subsystem with the CommandScheduler, allowing its
+   * Periodic() method to be called when the scheduler runs.
+   */
+  void Register();
+};
+}  // namespace frc2

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

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+
+#include <string>
+
+#include "Subsystem.h"
+
+namespace frc2 {
+/**
+ * A base for subsystems that handles registration in the constructor, and
+ * provides a more intuitive method for setting the default command.
+ */
+class SubsystemBase : public Subsystem, public frc::Sendable {
+ public:
+  void InitSendable(frc::SendableBuilder& builder) override;
+  std::string GetName() const override;
+  void SetName(const wpi::Twine& name) override;
+  std::string GetSubsystem() const override;
+  void SetSubsystem(const wpi::Twine& name) override;
+  void AddChild(std::string name, frc::Sendable* child);
+
+ protected:
+  SubsystemBase();
+  std::string m_name;
+};
+}  // namespace frc2

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc/Timer.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but takes a specified amount of time to finish.
+ * Useful for CommandGroups.  Can also be subclassed to make a command with an
+ * internal timer.
+ */
+class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
+ public:
+  /**
+   * Creates a new WaitCommand.  This command will do nothing, and end after the
+   * specified duration.
+   *
+   * @param seconds the time to wait, in seconds
+   */
+  explicit WaitCommand(double seconds);
+
+  WaitCommand(WaitCommand&& other) = default;
+
+  WaitCommand(const WaitCommand& other) = default;
+
+  void Initialize() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ protected:
+  std::unique_ptr<frc::Timer> m_timer;
+
+ private:
+  double m_duration;
+};
+}  // namespace frc2

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "frc/Timer.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but ends after a specified match time or
+ * condition.  Useful for CommandGroups.
+ */
+class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
+ public:
+  /**
+   * Creates a new WaitUntilCommand that ends after a given condition becomes
+   * true.
+   *
+   * @param condition the condition to determine when to end
+   */
+  explicit WaitUntilCommand(std::function<bool()> condition);
+
+  /**
+   * Creates a new WaitUntilCommand that ends after a given match time.
+   *
+   * <p>NOTE: The match timer used for this command is UNOFFICIAL.  Using this
+   * command does NOT guarantee that the time at which the action is performed
+   * will be judged to be legal by the referees.  When in doubt, add a safety
+   * factor or time the action manually.
+   *
+   * @param time the match time after which to end, in seconds
+   */
+  explicit WaitUntilCommand(double time);
+
+  WaitUntilCommand(WaitUntilCommand&& other) = default;
+
+  WaitUntilCommand(const WaitUntilCommand& other) = default;
+
+  bool IsFinished() override;
+
+  bool RunsWhenDisabled() const override;
+
+ private:
+  std::function<bool()> m_condition;
+};
+}  // namespace frc2

wpilibc/src/main/native/include/frc2/command/button/Button.h

@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <utility>
+
+#include "Trigger.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to button presses.  Can be composed
+ * with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class Button : public Trigger {
+ public:
+  /**
+   * Create a new button that is pressed when the given condition is true.
+   *
+   * @param isActive Whether the button is pressed.
+   */
+  explicit Button(std::function<bool()> isPressed);
+
+  /**
+   * Create a new button that is pressed active (default constructor) - activity
+   *  can be further determined by subclass code.
+   */
+  Button() = default;
+
+  /**
+   * Binds a command to start when the button is pressed.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Button WhenPressed(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Button WhenPressed(T&& command, bool interruptible = true) {
+    WhenActive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhenPressed(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started repeatedly while the button is pressed, and
+   * cancelled when it is released.  Takes a raw pointer, and so is non-owning;
+   * users are responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhileHeld(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started repeatedly while the button is pressed, and
+   * cancelled when it is released.  Transfers command ownership to the button
+   * scheduler, so the user does not have to worry about lifespan - rvalue refs
+   * will be *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Button WhileHeld(T&& command, bool interruptible = true) {
+    WhileActiveContinous(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute repeatedly while the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhileHeld(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started when the button is pressed, and cancelled
+   * when it is released.  Takes a raw pointer, and so is non-owning; users are
+   * responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhenHeld(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started when the button is pressed, and cancelled
+   * when it is released.  Transfers command ownership to the button scheduler,
+   * so the user does not have to worry about lifespan - rvalue refs will be
+   * *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Button WhenHeld(T&& command, bool interruptible = true) {
+    WhileActiveOnce(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a command to start when the button is released.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button WhenReleased(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Button WhenReleased(T&& command, bool interruptible = true) {
+    WhenInactive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the button is released.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Button WhenReleased(std::function<void()> toRun);
+
+  /**
+   * Binds a command to start when the button is pressed, and be cancelled when
+   * it is pressed again.  Takes a raw pointer, and so is non-owning; users are
+   * responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  Button ToggleWhenPressed(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the button is pressed, and be cancelled when
+   * it is pessed again.  Transfers command ownership to the button scheduler,
+   * so the user does not have to worry about lifespan - rvalue refs will be
+   * *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The button, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Button ToggleWhenPressed(T&& command, bool interruptible = true) {
+    ToggleWhenActive(std::forward<T>(command), interruptible);
+    return *this;
+  }
+
+  /**
+   * Binds a command to be cancelled when the button is pressed.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   *  and scheduling of the command.
+   *
+   * @param command The command to bind.
+   * @return The button, for chained calls.
+   */
+  Button CancelWhenPressed(Command* command);
+};
+}  // namespace frc2

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick button presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class JoystickButton : public Button {
+ public:
+  /**
+   * Creates a JoystickButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param buttonNumber The number of the button on the joystic.
+   */
+  explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
+      : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
+
+  bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
+
+ private:
+  frc::GenericHID* m_joystick;
+  int m_buttonNumber;
+};
+}  // namespace frc2

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick POV presses.  Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class POVButton : public Button {
+ public:
+  /**
+   * Creates a POVButton that commands can be bound to.
+   *
+   * @param joystick The joystick on which the button is located.
+   * @param angle The angle of the POV corresponding to a button press.
+   * @param povNumber The number of the POV on the joystick.
+   */
+  POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
+      : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
+
+  bool Get() const override {
+    return m_joystick->GetPOV(m_povNumber) == m_angle;
+  }
+
+ private:
+  frc::GenericHID* m_joystick;
+  int m_angle;
+  int m_povNumber;
+};
+}  // namespace frc2

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

@@ -0,0 +1,325 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandScheduler.h>
+
+#include <atomic>
+#include <memory>
+#include <utility>
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to events.  The
+ * Trigger class is a base for all command-event-binding classes, and so the
+ * methods are named fairly abstractly; for purpose-specific wrappers, see
+ * Button.
+ *
+ * @see Button
+ */
+class Trigger {
+ public:
+  /**
+   * Create a new trigger that is active when the given condition is true.
+   *
+   * @param isActive Whether the trigger is active.
+   */
+  explicit Trigger(std::function<bool()> isActive)
+      : m_isActive{std::move(isActive)} {}
+
+  /**
+   * Create a new trigger that is never active (default constructor) - activity
+   *  can be further determined by subclass code.
+   */
+  Trigger() {
+    m_isActive = [] { return false; };
+  }
+
+  Trigger(const Trigger& other);
+
+  /**
+   * Returns whether the trigger is active.  Can be overridden by a subclass.
+   *
+   * @return Whether the trigger is active.
+   */
+  virtual bool Get() const { return m_isActive(); }
+
+  /**
+   * Binds a command to start when the trigger becomes active.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhenActive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes active.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Trigger WhenActive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            command->Schedule(interruptible);
+          }
+
+          pressedLast = pressed;
+        });
+
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the trigger becomes active.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Trigger WhenActive(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started repeatedly while the trigger is active, and
+   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
+   * non-owning; users are responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhileActiveContinous(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started repeatedly while the trigger is active, and
+   * cancelled when it becomes inactive.  Transfers command ownership to the
+   * button scheduler, so the user does not have to worry about lifespan -
+   * rvalue refs will be *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (pressed) {
+            command->Schedule(interruptible);
+          } else if (pressedLast && !pressed) {
+            command->Cancel();
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute repeatedly while the trigger is active.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Trigger WhileActiveContinous(std::function<void()> toRun);
+
+  /**
+   * Binds a command to be started when the trigger becomes active, and
+   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
+   * non-owning; users are responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhileActiveOnce(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to be started when the trigger becomes active, and
+   * cancelled when it becomes inactive.  Transfers command ownership to the
+   * button scheduler, so the user does not have to worry about lifespan -
+   * rvalue refs will be *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            command->Schedule(interruptible);
+          } else if (pressedLast && !pressed) {
+            command->Cancel();
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a command to start when the trigger becomes inactive.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   * of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Trigger WhenInactive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes inactive.  Transfers
+   * command ownership to the button scheduler, so the user does not have to
+   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+   * *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Trigger WhenInactive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (pressedLast && !pressed) {
+            command->Schedule(interruptible);
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a runnable to execute when the trigger becomes inactive.
+   *
+   * @param toRun the runnable to execute.
+   */
+  Trigger WhenInactive(std::function<void()> toRun);
+
+  /**
+   * Binds a command to start when the trigger becomes active, and be cancelled
+   * when it again becomes active.  Takes a raw pointer, and so is non-owning;
+   * users are responsible for the lifespan of the command.
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  Trigger ToggleWhenActive(Command* command, bool interruptible = true);
+
+  /**
+   * Binds a command to start when the trigger becomes active, and be cancelled
+   * when it again becomes active.  Transfers command ownership to the button
+   * scheduler, so the user does not have to worry about lifespan - rvalue refs
+   * will be *moved*, lvalue refs will be *copied.*
+   *
+   * @param command The command to bind.
+   * @param interruptible Whether the command should be interruptible.
+   * @return The trigger, for chained calls.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of<
+                         Command, std::remove_reference_t<T>>::value>>
+  Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
+    CommandScheduler::GetInstance().AddButton(
+        [pressedLast = Get(), *this,
+         command = std::make_unique<std::remove_reference_t<T>>(
+             std::forward<T>(command)),
+         interruptible]() mutable {
+          bool pressed = Get();
+
+          if (!pressedLast && pressed) {
+            if (command->IsScheduled()) {
+              command->Cancel();
+            } else {
+              command->Schedule(interruptible);
+            }
+          }
+
+          pressedLast = pressed;
+        });
+    return *this;
+  }
+
+  /**
+   * Binds a command to be cancelled when the trigger becomes active.  Takes a
+   * raw pointer, and so is non-owning; users are responsible for the lifespan
+   *  and scheduling of the command.
+   *
+   * @param command The command to bind.
+   * @return The trigger, for chained calls.
+   */
+  Trigger CancelWhenActive(Command* command);
+
+  /**
+   * Composes two triggers with logical AND.
+   *
+   * @return A trigger which is active when both component triggers are active.
+   */
+  Trigger operator&&(Trigger rhs) {
+    return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+  }
+
+  /**
+   * Composes two triggers with logical OR.
+   *
+   * @return A trigger which is active when either component trigger is active.
+   */
+  Trigger operator||(Trigger rhs) {
+    return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+  }
+
+  /**
+   * Composes a trigger with logical NOT.
+   *
+   * @return A trigger which is active when the component trigger is inactive,
+   * and vice-versa.
+   */
+  Trigger operator!() {
+    return Trigger([*this] { return !Get(); });
+  }
+
+ private:
+  std::function<bool()> m_isActive;
+};
+}  // namespace frc2

wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp

@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenReleasedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = true;
+  Trigger([&pressed] { return pressed; }).WhenInactive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhileHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  pressed = true;
+  scheduler.Run();
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, AndTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) &&
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, OrTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command1([&finished] { return finished; });
+  WaitUntilCommand command2([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command1);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+  pressed1 = false;
+
+  (Trigger([&pressed1] { return pressed1; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command2);
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, NegateTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButtonTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int counter = 0;
+  bool pressed = false;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(counter, 0);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(counter, 1);
+}

wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = RunCommand([] {}, {}).WithTimeout(.1);
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, InterruptOnTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      RunCommand([] {}, {}).InterruptOn([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  finished = true;
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = InstantCommand([] {}, {}).BeforeStarting(
+      [&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, WhenFinishedTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      InstantCommand([] {}, {}).WhenFinished([&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_FALSE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(finished);
+}
+
+TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = InstantCommand([] {}, {}).Perpetually();
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp

@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute());
+  EXPECT_CALL(command1, End(true));
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute());
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(&command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(true)).Times(0);
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  EXPECT_CALL(command2, Initialize()).Times(0);
+  EXPECT_CALL(command2, Execute()).Times(0);
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(false, &command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_FALSE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+  TestSubsystem requirement1;
+  ErrorConfirmer confirmer("require");
+
+  MockCommand command1;
+
+  requirement1.SetDefaultCommand(std::move(command1));
+
+  EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+}

wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp

@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false));
+
+  command.SetFinished(true);
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute()).Times(2);
+  EXPECT_CALL(command, End(false));
+
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  command.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, MultiScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command1;
+  MockCommand command2;
+  MockCommand command3;
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(false));
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute()).Times(3);
+  EXPECT_CALL(command2, End(false));
+
+  EXPECT_CALL(command3, Initialize());
+  EXPECT_CALL(command3, Execute()).Times(4);
+  EXPECT_CALL(command3, End(false));
+
+  scheduler.Schedule(&command1);
+  scheduler.Schedule(&command2);
+  scheduler.Schedule(&command3);
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  command1.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  command2.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
+  command3.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
+}
+
+TEST_F(CommandScheduleTest, SchedulerCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false)).Times(0);
+  EXPECT_CALL(command, End(true));
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Cancel(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+  auto& scheduler = CommandScheduler::GetInstance();
+  scheduler.CancelAll();
+  scheduler.Enable();
+  scheduler.ClearButtons();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+
+void CommandTestBase::SetUp() {
+  HALSIM_SetDriverStationEnabled(true);
+  while (!HALSIM_GetDriverStationEnabled()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
+
+void CommandTestBase::TearDown() {
+  CommandScheduler::GetInstance().ClearButtons();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+  HALSIM_SetDriverStationEnabled(enabled);
+  while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}

wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <regex>
+#include <utility>
+
+#include <mockdata/MockHooks.h>
+
+#include "ErrorConfirmer.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/SetUtilities.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "make_vector.h"
+#include "simulation/DriverStationSim.h"
+
+namespace frc2 {
+class CommandTestBase : public ::testing::Test {
+ public:
+  CommandTestBase();
+
+  class TestSubsystem : public SubsystemBase {};
+
+ protected:
+  class MockCommand : public Command {
+   public:
+    MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+    MOCK_METHOD0(IsFinished, bool());
+    MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+    MOCK_METHOD0(Initialize, void());
+    MOCK_METHOD0(Execute, void());
+    MOCK_METHOD1(End, void(bool interrupted));
+
+    MockCommand() {
+      m_requirements = {};
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+      EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(true));
+    }
+
+    MockCommand(std::initializer_list<Subsystem*> requirements,
+                bool finished = false, bool runWhenDisabled = true) {
+      m_requirements.insert(requirements.begin(), requirements.end());
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(finished));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(runWhenDisabled));
+    }
+
+    MockCommand(MockCommand&& other) {
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(other.IsFinished()));
+      EXPECT_CALL(*this, RunsWhenDisabled())
+          .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+      std::swap(m_requirements, other.m_requirements);
+      EXPECT_CALL(*this, GetRequirements())
+          .WillRepeatedly(::testing::Return(m_requirements));
+    }
+
+    MockCommand(const MockCommand& other) : Command{} {}
+
+    void SetFinished(bool finished) {
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(finished));
+    }
+
+    ~MockCommand() {
+      auto& scheduler = CommandScheduler::GetInstance();
+      scheduler.Cancel(this);
+    }
+
+   protected:
+    std::unique_ptr<Command> TransferOwnership() && {
+      return std::make_unique<MockCommand>(std::move(*this));
+    }
+
+   private:
+    wpi::SmallSet<Subsystem*, 4> m_requirements;
+  };
+
+  CommandScheduler GetScheduler();
+
+  void SetUp() override;
+
+  void TearDown() override;
+
+  void SetDSEnabled(bool enabled);
+};
+}  // namespace frc2

wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  ConditionalCommand conditional(
+      std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
+
+  scheduler.Schedule(&conditional);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ConditionalCommand conditional(std::move(command1), std::move(command2),
+                                 [] { return true; });
+  scheduler.Schedule(&conditional);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}

wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+  RunCommand command2([] {}, {&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+  scheduler.Schedule(&command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+  EXPECT_FALSE(scheduler.IsScheduled(handle));
+
+  scheduler.Cancel(&command2);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}

wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ErrorConfirmer.h"
+
+ErrorConfirmer* ErrorConfirmer::instance;
+
+int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
+                                    HAL_Bool isLVCode, const char* details,
+                                    const char* location, const char* callStack,
+                                    HAL_Bool printMsg) {
+  if (std::regex_search(details, std::regex(instance->m_msg))) {
+    instance->ConfirmError();
+  }
+  return 1;
+}

wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <regex>
+
+#include <mockdata/MockHooks.h>
+
+#include "gmock/gmock.h"
+#include "simulation/DriverStationSim.h"
+
+class ErrorConfirmer {
+ public:
+  explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
+    if (instance != nullptr) return;
+    HALSIM_SetSendError(HandleError);
+    EXPECT_CALL(*this, ConfirmError());
+    instance = this;
+  }
+
+  ~ErrorConfirmer() {
+    HALSIM_SetSendError(nullptr);
+    instance = nullptr;
+  }
+
+  MOCK_METHOD0(ConfirmError, void());
+
+  const char* m_msg;
+
+  static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
+                             HAL_Bool isLVCode, const char* details,
+                             const char* location, const char* callStack,
+                             HAL_Bool printMsg);
+
+ private:
+  static ErrorConfirmer* instance;
+};

wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+  bool finished = false;
+
+  FunctionalCommand command(
+      [&counter] { counter++; }, [&counter] { counter++; },
+      [&counter](bool) { counter++; }, [&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(4, counter);
+}

wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  InstantCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(counter, 1);
+}

wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+class NotifierCommandTest : public CommandTestBase {};
+
+#ifdef __APPLE__
+TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
+#else
+TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+#endif
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  NotifierCommand command([&counter] { counter++; }, 0.01, {});
+
+  scheduler.Schedule(&command);
+  std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  scheduler.Cancel(&command);
+
+  EXPECT_NEAR(.01 * counter, .25, .025);
+}

wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelCommandGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&group);
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup group((InstantCommand(), InstantCommand()));
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp

@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelDeadlineGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                                 std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  command2->SetFinished(true);
+  scheduler.Run();
+  command1->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                                 std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelDeadlineGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelDeadlineGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp

@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelRaceGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelRaceGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+class PerpetualCommandTest : public CommandTestBase {};
+
+TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool check = false;
+
+  PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(check);
+}

wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  PrintCommand command("Test!");
+
+  testing::internal::CaptureStdout();
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
+                                std::regex("Test!")));
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool finished = false;
+
+  WaitUntilCommand toSchedule([&finished] { return finished; });
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp

@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class RobotDisabledCommandTest : public CommandTestBase {};
+
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command({}, false, false);
+
+  EXPECT_CALL(command, End(true));
+
+  SetDSEnabled(true);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command1;
+  MockCommand command2;
+
+  scheduler.Schedule(&command1);
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  scheduler.Schedule(&command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
+                                             MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
+                                           MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
+                                        MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
+                                            MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
+                                     [] { return true; }};
+  ConditionalCommand dontRunWhenDisabled{
+      MockCommand(), MockCommand({}, false, false), [] { return true; }};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SelectCommand<int> runWhenDisabled{[] { return 1; },
+                                     std::pair(1, MockCommand()),
+                                     std::pair(1, MockCommand())};
+  SelectCommand<int> dontRunWhenDisabled{
+      [] { return 1; }, std::pair(1, MockCommand()),
+      std::pair(1, MockCommand({}, false, false))};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}

wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_EQ(3, counter);
+}

wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class SchedulerTest : public CommandTestBase {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  InstantCommand command;
+
+  int counter = 0;
+
+  scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
+  scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
+  scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_EQ(counter, 3);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  RunCommand command([] {}, {});
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem system;
+
+  scheduler.RegisterSubsystem(&system);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+}

wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommandTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
+
+  temp.emplace_back(std::pair(1, std::move(mock)));
+  temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
+  temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
+
+  SelectCommand<int> select([] { return 1; }, std::move(temp));
+
+  scheduler.Schedule(&select);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
+
+TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SelectCommand<int> select([] { return 1; }, std::pair(1, std::move(command1)),
+                            std::pair(2, std::move(command2)));
+
+  scheduler.Schedule(&select);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&select));
+}

wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp

@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class SequentialCommandGroupTest : public CommandTestBase {};
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+  command3->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(0);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize()).Times(0);
+  EXPECT_CALL(*command3, Execute()).Times(0);
+  EXPECT_CALL(*command3, End(false)).Times(0);
+  EXPECT_CALL(*command3, End(true)).Times(0);
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  SequentialCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SequentialCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
+                          {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(2, counter);
+}

wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  WaitCommand command(.1);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibc/src/test/native/cpp/frc2/command/make_vector.h

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace tcb {
+
+namespace detail {
+
+template <typename T, typename...>
+struct vec_type_helper {
+  using type = T;
+};
+
+template <typename... Args>
+struct vec_type_helper<void, Args...> {
+  using type = typename std::common_type<Args...>::type;
+};
+
+template <typename T, typename... Args>
+using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
+
+template <typename, typename...>
+struct all_constructible_and_convertible : std::true_type {};
+
+template <typename T, typename First, typename... Rest>
+struct all_constructible_and_convertible<T, First, Rest...>
+    : std::conditional<std::is_constructible<T, First>::value &&
+                           std::is_convertible<First, T>::value,
+                       all_constructible_and_convertible<T, Rest...>,
+                       std::false_type>::type {};
+
+template <typename T, typename... Args,
+          typename std::enable_if<!std::is_trivially_copyable<T>::value,
+                                  int>::type = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  std::vector<T> vec;
+  vec.reserve(sizeof...(Args));
+  using arr_t = int[];
+  (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+  return vec;
+}
+
+template <typename T, typename... Args,
+          typename std::enable_if<std::is_trivially_copyable<T>::value,
+                                  int>::type = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  return std::vector<T>{std::forward<Args>(args)...};
+}
+
+}  // namespace detail
+
+template <typename T = void, typename... Args,
+          typename V = detail::vec_type_helper_t<T, Args...>,
+          typename std::enable_if<
+              detail::all_constructible_and_convertible<V, Args...>::value,
+              int>::type = 0>
+std::vector<V> make_vector(Args&&... args) {
+  return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+}
+
+}  // namespace tcb

wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp

@@ -7,7 +7,7 @@
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/trajectory/TrapezoidProfile.h>
@@ -32,7 +32,7 @@ class Robot : public frc::TimedRobot {
  private:
   frc::Joystick m_joystick{1};
   frc::Encoder m_encoder{1, 2};
-  frc::Spark m_motor{1};
+  frc::PWMVictorSPX m_motor{1};
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.

wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp

@@ -7,7 +7,7 @@
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
 #include <frc/trajectory/TrapezoidProfile.h>
@@ -43,7 +43,7 @@ class Robot : public frc::TimedRobot {
  private:
   frc::Joystick m_joystick{1};
   frc::Encoder m_encoder{1, 2};
-  frc::Spark m_motor{1};
+  frc::PWMVictorSPX m_motor{1};
   frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
 
   frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp

@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc2/command/button/JoystickButton.h>
+
+#include <frc/shuffleboard/Shuffleboard.h>
+
+RobotContainer::RobotContainer() {
+  // Initialize all of your commands and subsystems here
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+
+  // Set up default drive command
+  m_drive.SetDefaultCommand(frc2::RunCommand(
+      [this] {
+        m_drive.ArcadeDrive(
+            m_driverController.GetY(frc::GenericHID::kLeftHand),
+            m_driverController.GetX(frc::GenericHID::kRightHand));
+      },
+      {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+
+  // Spin up the shooter when the 'A' button is pressed
+  frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter);
+
+  // Turn off the shooter when the 'B' button is pressed
+  frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter);
+
+  // Shoot when the 'X' button is held
+  frc2::JoystickButton(&m_driverController, 3)
+      .WhenPressed(&m_shoot)
+      .WhenReleased(&m_stopFeeder);
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed(&m_driveHalfSpeed)
+      .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Runs the chosen command in autonomous
+  return &m_autonomousCommand;
+}

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_left1{kLeftMotor1Port},
+      m_left2{kLeftMotor2Port},
+      m_right1{kRightMotor1Port},
+      m_right2{kRightMotor2Port},
+      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // Set the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/ShooterSubsystem.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Constants.h"
+
+using namespace ShooterConstants;
+
+ShooterSubsystem::ShooterSubsystem()
+    : PIDSubsystem(frc2::PIDController(kP, kI, kD)),
+      m_shooterMotor(kShooterMotorPort),
+      m_feederMotor(kFeederMotorPort),
+      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
+  m_controller.SetAbsoluteTolerance(kShooterToleranceRPS);
+  m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void ShooterSubsystem::UseOutput(double output) {
+  // Use a feedforward of the form kS + kV * velocity
+  m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
+}
+
+void ShooterSubsystem::Disable() {
+  // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
+  // due to our feedforward
+  frc2::PIDSubsystem::Disable();
+  m_shooterMotor.Set(0);
+}
+
+bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
+
+double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
+
+double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
+
+void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
+
+void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+}  // namespace DriveConstants
+
+namespace ShooterConstants {
+const int kEncoderPorts[]{4, 5};
+const bool kEncoderReversed = false;
+const int kEncoderCPR = 1024;
+const double kEncoderDistancePerPulse =
+    // Distance units will be rotations
+    1. / static_cast<double>(kEncoderCPR);
+
+const int kShooterMotorPort = 4;
+const int kFeederMotorPort = 5;
+
+const double kShooterFreeRPS = 5300;
+const double kShooterTargetRPS = 4000;
+const double kShooterToleranceRPS = 50;
+
+const double kP = 1;
+const double kI = 0;
+const double kD = 0;
+
+// On a real robot the feedforward constants should be empirically determined;
+// these are reasonable guesses.
+const double kSFractional = .05;
+const double kVFractional =
+    // Should have value 1 at free speed...
+    1. / kShooterFreeRPS;
+
+const double kFeederSpeed = .5;
+}  // namespace ShooterConstants
+
+namespace AutoConstants {
+const double kAutoTimeoutSeconds = 12;
+const double kAutoShootTimeSeconds = 7;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+}  // namespace OIConstants

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+
+#include <frc/TimedRobot.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+#include "subsystems/ShooterSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // The driver's controller
+  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+  // The robot's subsystems and commands are defined here...
+
+  // The robot's subsystems
+  DriveSubsystem m_drive;
+  ShooterSubsystem m_shooter;
+
+  // A simple autonomous routine that shoots the loaded frisbees
+  frc2::SequentialCommandGroup m_autonomousCommand =
+      frc2::SequentialCommandGroup{
+          // Start the command by spinning up the shooter...
+          frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}),
+          // Wait until the shooter is at speed before feeding the frisbees
+          frc2::WaitUntilCommand([this] { return m_shooter.AtSetpoint(); }),
+          // Start running the feeder
+          frc2::InstantCommand([this] { m_shooter.RunFeeder(); }, {&m_shooter}),
+          // Shoot for the specified time
+          frc2::WaitCommand(ac::kAutoShootTimeSeconds)}
+          // Add a timeout (will end the command if, for instance, the shooter
+          // never gets up to
+          // speed)
+          .WithTimeout(ac::kAutoTimeoutSeconds)
+          // When the command ends, turn off the shooter and the feeder
+          .WhenFinished([this] {
+            m_shooter.Disable();
+            m_shooter.StopFeeder();
+          });
+
+  // Assorted commands to be bound to buttons
+
+  frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); },
+                                       {&m_shooter}};
+
+  frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); },
+                                     {&m_shooter}};
+
+  // Shoots if the shooter wheen has reached the target speed
+  frc2::ConditionalCommand m_shoot{
+      // Run the feeder
+      frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}},
+      // Do nothing
+      frc2::InstantCommand(),
+      // Determine which of the above to do based on whether the shooter has
+      // reached the
+      // desired speed
+      [this] { return m_shooter.AtSetpoint(); }};
+
+  frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
+                                    {&m_shooter}};
+
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+                                        {}};
+  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+                                        {}};
+
+  // The chooser for the autonomous routines
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/SubsystemBase.h>
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  // Subsystem methods go here.
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  double GetAverageEncoderDistance();
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  frc::Encoder& GetLeftEncoder();
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  frc::Encoder& GetRightEncoder();
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive
+   * more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  void SetMaxOutput(double maxOutput);
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  // The motor controllers
+  frc::PWMVictorSPX m_left1;
+  frc::PWMVictorSPX m_left2;
+  frc::PWMVictorSPX m_right1;
+  frc::PWMVictorSPX m_right2;
+
+  // The motors on the left side of the drive
+  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+  // The motors on the right side of the drive
+  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+  // The left-side drive encoder
+  frc::Encoder m_leftEncoder;
+
+  // The right-side drive encoder
+  frc::Encoder m_rightEncoder;
+};

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/PIDSubsystem.h>
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+
+class ShooterSubsystem : public frc2::PIDSubsystem {
+ public:
+  ShooterSubsystem();
+
+  void UseOutput(double output) override;
+
+  double GetSetpoint() override;
+
+  double GetMeasurement() override;
+
+  void Disable() override;
+
+  bool AtSetpoint();
+
+  void RunFeeder();
+
+  void StopFeeder();
+
+ private:
+  frc::PWMVictorSPX m_shooterMotor;
+  frc::PWMVictorSPX m_feederMotor;
+  frc::Encoder m_shooterEncoder;
+};