Split the two command implementations into separate libraries (#2012)

This will allow us at the user code side to determine to include old commands, new commands or both.

wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+
+using namespace frc;
+
+void Button::WhenPressed(Command* command) { WhenActive(command); }
+
+void Button::WhileHeld(Command* command) { WhileActive(command); }
+
+void Button::WhenReleased(Command* command) { WhenInactive(command); }
+
+void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
+
+void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }

wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ButtonScheduler.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
+    : m_pressedLast(last), m_button(button), m_command(orders) {}
+
+void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }

wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/CancelButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
+                                             Command* orders)
+    : ButtonScheduler(last, button, orders) {}
+
+void CancelButtonScheduler::Execute() {
+  bool pressed = m_button->Grab();
+
+  if (!m_pressedLast && pressed) {
+    m_command->Cancel();
+  }
+
+  m_pressedLast = pressed;
+}

wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/HeldButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
+                                         Command* orders)
+    : ButtonScheduler(last, button, orders) {}
+
+void HeldButtonScheduler::Execute() {
+  bool pressed = m_button->Grab();
+
+  if (pressed) {
+    m_command->Start();
+  } else if (m_pressedLast && !pressed) {
+    m_command->Cancel();
+  }
+
+  m_pressedLast = pressed;
+}

wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/InternalButton.h"
+
+using namespace frc;
+
+InternalButton::InternalButton(bool inverted)
+    : m_pressed(inverted), m_inverted(inverted) {}
+
+void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
+
+void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
+
+bool InternalButton::Get() { return m_pressed ^ m_inverted; }

wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp

@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/JoystickButton.h"
+
+using namespace frc;
+
+JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
+    : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
+
+bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }

wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/NetworkButton.h"
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
+using namespace frc;
+
+NetworkButton::NetworkButton(const wpi::Twine& tableName,
+                             const wpi::Twine& field)
+    : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
+                    field) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                             const wpi::Twine& field)
+    : m_entry(table->GetEntry(field)) {}
+
+bool NetworkButton::Get() {
+  return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
+}

wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp

@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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 "frc/buttons/POVButton.h"
+
+using namespace frc;
+
+POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
+    : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
+
+bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }

wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/PressedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
+                                               Command* orders)
+    : ButtonScheduler(last, button, orders) {}
+
+void PressedButtonScheduler::Execute() {
+  bool pressed = m_button->Grab();
+
+  if (!m_pressedLast && pressed) {
+    m_command->Start();
+  }
+
+  m_pressedLast = pressed;
+}

wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ReleasedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
+                                                 Command* orders)
+    : ButtonScheduler(last, button, orders) {}
+
+void ReleasedButtonScheduler::Execute() {
+  bool pressed = m_button->Grab();
+
+  if (m_pressedLast && !pressed) {
+    m_command->Start();
+  }
+
+  m_pressedLast = pressed;
+}

wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ToggleButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
+                                             Command* orders)
+    : ButtonScheduler(last, button, orders) {}
+
+void ToggleButtonScheduler::Execute() {
+  bool pressed = m_button->Grab();
+
+  if (!m_pressedLast && pressed) {
+    if (m_command->IsRunning()) {
+      m_command->Cancel();
+    } else {
+      m_command->Start();
+    }
+  }
+
+  m_pressedLast = pressed;
+}

wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+#include "frc/buttons/CancelButtonScheduler.h"
+#include "frc/buttons/HeldButtonScheduler.h"
+#include "frc/buttons/PressedButtonScheduler.h"
+#include "frc/buttons/ReleasedButtonScheduler.h"
+#include "frc/buttons/ToggleButtonScheduler.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
+
+Trigger& Trigger::operator=(const Trigger& rhs) {
+  SendableHelper::operator=(rhs);
+  m_sendablePressed = false;
+  return *this;
+}
+
+Trigger::Trigger(Trigger&& rhs)
+    : SendableHelper(std::move(rhs)),
+      m_sendablePressed(rhs.m_sendablePressed.load()) {
+  rhs.m_sendablePressed = false;
+}
+
+Trigger& Trigger::operator=(Trigger&& rhs) {
+  SendableHelper::operator=(std::move(rhs));
+  m_sendablePressed = rhs.m_sendablePressed.load();
+  rhs.m_sendablePressed = false;
+  return *this;
+}
+
+bool Trigger::Grab() { return Get() || m_sendablePressed; }
+
+void Trigger::WhenActive(Command* command) {
+  auto pbs = new PressedButtonScheduler(Grab(), this, command);
+  pbs->Start();
+}
+
+void Trigger::WhileActive(Command* command) {
+  auto hbs = new HeldButtonScheduler(Grab(), this, command);
+  hbs->Start();
+}
+
+void Trigger::WhenInactive(Command* command) {
+  auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
+  rbs->Start();
+}
+
+void Trigger::CancelWhenActive(Command* command) {
+  auto cbs = new CancelButtonScheduler(Grab(), this, command);
+  cbs->Start();
+}
+
+void Trigger::ToggleWhenActive(Command* command) {
+  auto tbs = new ToggleButtonScheduler(Grab(), this, command);
+  tbs->Start();
+}
+
+void Trigger::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Button");
+  builder.SetSafeState([=]() { m_sendablePressed = false; });
+  builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
+                             [=](bool value) { m_sendablePressed = value; });
+}

wpilibOldCommands/src/main/native/cpp/commands/Command.cpp

@@ -0,0 +1,264 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Command.h"
+
+#include <typeinfo>
+
+#include "frc/RobotState.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/commands/CommandGroup.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+int Command::m_commandCounter = 0;
+
+Command::Command() : Command("", -1.0) {}
+
+Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
+
+Command::Command(double timeout) : Command("", timeout) {}
+
+Command::Command(Subsystem& subsystem) : Command("", -1.0) {
+  Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout) {
+  // We use -1.0 to indicate no timeout.
+  if (timeout < 0.0 && timeout != -1.0)
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+
+  m_timeout = timeout;
+
+  // If name contains an empty string
+  if (name.isTriviallyEmpty() ||
+      (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
+    SendableRegistry::GetInstance().Add(
+        this, "Command_" + wpi::Twine(typeid(*this).name()));
+  } else {
+    SendableRegistry::GetInstance().Add(this, name);
+  }
+}
+
+Command::Command(const wpi::Twine& name, Subsystem& subsystem)
+    : Command(name, -1.0) {
+  Requires(&subsystem);
+}
+
+Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
+  Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
+    : Command(name, timeout) {
+  Requires(&subsystem);
+}
+
+double Command::TimeSinceInitialized() const {
+  if (m_startTime < 0.0)
+    return 0.0;
+  else
+    return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+void Command::Requires(Subsystem* subsystem) {
+  if (!AssertUnlocked("Can not add new requirement to command")) return;
+
+  if (subsystem != nullptr)
+    m_requirements.insert(subsystem);
+  else
+    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+void Command::Start() {
+  LockChanges();
+  if (m_parent != nullptr)
+    wpi_setWPIErrorWithContext(
+        CommandIllegalUse,
+        "Can not start a command that is part of a command group");
+
+  m_completed = false;
+  Scheduler::GetInstance()->AddCommand(this);
+}
+
+bool Command::Run() {
+  if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+    Cancel();
+
+  if (IsCanceled()) return false;
+
+  if (!m_initialized) {
+    m_initialized = true;
+    StartTiming();
+    _Initialize();
+    Initialize();
+  }
+  _Execute();
+  Execute();
+  return !IsFinished();
+}
+
+void Command::Cancel() {
+  if (m_parent != nullptr)
+    wpi_setWPIErrorWithContext(
+        CommandIllegalUse,
+        "Can not cancel a command that is part of a command group");
+
+  _Cancel();
+}
+
+bool Command::IsRunning() const { return m_running; }
+
+bool Command::IsInitialized() const { return m_initialized; }
+
+bool Command::IsCompleted() const { return m_completed; }
+
+bool Command::IsCanceled() const { return m_canceled; }
+
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+void Command::SetInterruptible(bool interruptible) {
+  m_interruptible = interruptible;
+}
+
+bool Command::DoesRequire(Subsystem* system) const {
+  return m_requirements.count(system) > 0;
+}
+
+const Command::SubsystemSet& Command::GetRequirements() const {
+  return m_requirements;
+}
+
+CommandGroup* Command::GetGroup() const { return m_parent; }
+
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+int Command::GetID() const { return m_commandID; }
+
+void Command::SetTimeout(double timeout) {
+  if (timeout < 0.0)
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+  else
+    m_timeout = timeout;
+}
+
+bool Command::IsTimedOut() const {
+  return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+bool Command::AssertUnlocked(const std::string& message) {
+  if (m_locked) {
+    std::string buf =
+        message + " after being started or being added to a command group";
+    wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+    return false;
+  }
+  return true;
+}
+
+void Command::SetParent(CommandGroup* parent) {
+  if (parent == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "parent");
+  } else if (m_parent != nullptr) {
+    wpi_setWPIErrorWithContext(CommandIllegalUse,
+                               "Can not give command to a command group after "
+                               "already being put in a command group");
+  } else {
+    LockChanges();
+    m_parent = parent;
+  }
+}
+
+bool Command::IsParented() const { return m_parent != nullptr; }
+
+void Command::ClearRequirements() { m_requirements.clear(); }
+
+void Command::Initialize() {}
+
+void Command::Execute() {}
+
+void Command::End() {}
+
+void Command::Interrupted() { End(); }
+
+void Command::_Initialize() { m_completed = false; }
+
+void Command::_Interrupted() { m_completed = true; }
+
+void Command::_Execute() {}
+
+void Command::_End() { m_completed = true; }
+
+void Command::_Cancel() {
+  if (IsRunning()) m_canceled = true;
+}
+
+void Command::LockChanges() { m_locked = true; }
+
+void Command::Removed() {
+  if (m_initialized) {
+    if (IsCanceled()) {
+      Interrupted();
+      _Interrupted();
+    } else {
+      End();
+      _End();
+    }
+  }
+  m_initialized = false;
+  m_canceled = false;
+  m_running = false;
+  m_completed = true;
+}
+
+void Command::StartRunning() {
+  m_running = true;
+  m_startTime = -1;
+  m_completed = false;
+}
+
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+std::string Command::GetName() const {
+  return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Command::SetName(const wpi::Twine& name) {
+  SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Command::GetSubsystem() const {
+  return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Command::SetSubsystem(const wpi::Twine& name) {
+  SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void Command::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(
+      ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
+      nullptr);
+  builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
+                             [=](bool value) {
+                               if (value) {
+                                 if (!IsRunning()) Start();
+                               } else {
+                                 if (IsRunning()) Cancel();
+                               }
+                             });
+  builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
+                             nullptr);
+}

wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp

@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroup.h"
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
+
+void CommandGroup::AddSequential(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddSequential(Command* command, double timeout) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+  if (timeout < 0.0) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+    return;
+  }
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
+                          timeout);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command, double timeout) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+  if (timeout < 0.0) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+    return;
+  }
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
+                          timeout);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+bool CommandGroup::IsInterruptible() const {
+  if (!Command::IsInterruptible()) return false;
+
+  if (m_currentCommandIndex != -1 &&
+      static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+    Command* cmd = m_commands[m_currentCommandIndex].m_command;
+    if (!cmd->IsInterruptible()) return false;
+  }
+
+  for (const auto& child : m_children) {
+    if (!child->m_command->IsInterruptible()) return false;
+  }
+
+  return true;
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
+
+void CommandGroup::Initialize() {}
+
+void CommandGroup::Execute() {}
+
+bool CommandGroup::IsFinished() {
+  return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
+         m_children.empty();
+}
+
+void CommandGroup::End() {}
+
+void CommandGroup::Interrupted() {}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+  CommandGroupEntry* entry;
+  Command* cmd = nullptr;
+  bool firstRun = false;
+
+  if (m_currentCommandIndex == -1) {
+    firstRun = true;
+    m_currentCommandIndex = 0;
+  }
+
+  // While there are still commands in this group to run
+  while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+    // If a command is prepared to run
+    if (cmd != nullptr) {
+      // If command timed out, cancel it so it's removed from the Scheduler
+      if (entry->IsTimedOut()) cmd->_Cancel();
+
+      // If command finished or was cancelled, remove it from Scheduler
+      if (cmd->Run()) {
+        break;
+      } else {
+        cmd->Removed();
+
+        // Advance to next command in group
+        m_currentCommandIndex++;
+        firstRun = true;
+        cmd = nullptr;
+        continue;
+      }
+    }
+
+    entry = &m_commands[m_currentCommandIndex];
+    cmd = nullptr;
+
+    switch (entry->m_state) {
+      case CommandGroupEntry::kSequence_InSequence:
+        cmd = entry->m_command;
+        if (firstRun) {
+          cmd->StartRunning();
+          CancelConflicts(cmd);
+          firstRun = false;
+        }
+        break;
+
+      case CommandGroupEntry::kSequence_BranchPeer:
+        // Start executing a parallel command and advance to next entry in group
+        m_currentCommandIndex++;
+        entry->m_command->Start();
+        break;
+
+      case CommandGroupEntry::kSequence_BranchChild:
+        m_currentCommandIndex++;
+
+        /* Causes scheduler to skip children of current command which require
+         * the same subsystems as it
+         */
+        CancelConflicts(entry->m_command);
+        entry->m_command->StartRunning();
+
+        // Add current command entry to list of children of this group
+        m_children.push_back(entry);
+        break;
+    }
+  }
+
+  // Run Children
+  for (auto& entry : m_children) {
+    auto child = entry->m_command;
+    if (entry->IsTimedOut()) {
+      child->_Cancel();
+    }
+
+    // If child finished or was cancelled, set it to nullptr. nullptr entries
+    // are removed later.
+    if (!child->Run()) {
+      child->Removed();
+      entry = nullptr;
+    }
+  }
+
+  m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
+                   m_children.end());
+}
+
+void CommandGroup::_End() {
+  // Theoretically, we don't have to check this, but we do if teams override the
+  // IsFinished method
+  if (m_currentCommandIndex != -1 &&
+      static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+    Command* cmd = m_commands[m_currentCommandIndex].m_command;
+    cmd->_Cancel();
+    cmd->Removed();
+  }
+
+  for (auto& child : m_children) {
+    Command* cmd = child->m_command;
+    cmd->_Cancel();
+    cmd->Removed();
+  }
+  m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+void CommandGroup::CancelConflicts(Command* command) {
+  for (auto childIter = m_children.begin(); childIter != m_children.end();) {
+    Command* child = (*childIter)->m_command;
+    bool erased = false;
+
+    for (auto&& requirement : command->GetRequirements()) {
+      if (child->DoesRequire(requirement)) {
+        child->_Cancel();
+        child->Removed();
+        childIter = m_children.erase(childIter);
+        erased = true;
+        break;
+      }
+    }
+    if (!erased) childIter++;
+  }
+}

wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp

@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroupEntry.h"
+
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
+                                     double timeout)
+    : m_timeout(timeout), m_command(command), m_state(state) {}
+
+bool CommandGroupEntry::IsTimedOut() const {
+  if (m_timeout < 0.0) return false;
+  double time = m_command->TimeSinceInitialized();
+  if (time == 0.0) return false;
+  return time >= m_timeout;
+}

wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp

@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/commands/ConditionalCommand.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
+  if (onTrue != nullptr) {
+    for (auto requirement : onTrue->GetRequirements())
+      command.Requires(requirement);
+  }
+  if (onFalse != nullptr) {
+    for (auto requirement : onFalse->GetRequirements())
+      command.Requires(requirement);
+  }
+}
+
+ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
+  m_onTrue = onTrue;
+  m_onFalse = onFalse;
+
+  RequireAll(*this, onTrue, onFalse);
+}
+
+ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+                                       Command* onFalse)
+    : Command(name) {
+  m_onTrue = onTrue;
+  m_onFalse = onFalse;
+
+  RequireAll(*this, onTrue, onFalse);
+}
+
+void ConditionalCommand::_Initialize() {
+  if (Condition()) {
+    m_chosenCommand = m_onTrue;
+  } else {
+    m_chosenCommand = m_onFalse;
+  }
+
+  if (m_chosenCommand != nullptr) {
+    // This is a hack to make cancelling the chosen command inside a
+    // CommandGroup work properly
+    m_chosenCommand->ClearRequirements();
+
+    m_chosenCommand->Start();
+  }
+  Command::_Initialize();
+}
+
+void ConditionalCommand::_Cancel() {
+  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+    m_chosenCommand->Cancel();
+  }
+
+  Command::_Cancel();
+}
+
+bool ConditionalCommand::IsFinished() {
+  if (m_chosenCommand != nullptr) {
+    return m_chosenCommand->IsCompleted();
+  } else {
+    return true;
+  }
+}
+
+void ConditionalCommand::_Interrupted() {
+  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+    m_chosenCommand->Cancel();
+  }
+
+  Command::_Interrupted();
+}

wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-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 "frc/commands/InstantCommand.h"
+
+using namespace frc;
+
+InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
+    : Command(name, subsystem) {}
+
+InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
+    : InstantCommand(subsystem) {
+  m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name,
+                               std::function<void()> func)
+    : InstantCommand(name) {
+  m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+                               std::function<void()> func)
+    : InstantCommand(name, subsystem) {
+  m_func = func;
+}
+
+void InstantCommand::_Initialize() {
+  Command::_Initialize();
+  if (m_func) {
+    m_func();
+  }
+}
+
+bool InstantCommand::IsFinished() { return true; }

wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDCommand.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double f, double period)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
+  m_controller =
+      std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double period)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double f, double period, Subsystem& subsystem)
+    : Command(name, subsystem) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
+                       Subsystem& subsystem)
+    : Command(subsystem) {
+  m_controller =
+      std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       Subsystem& subsystem)
+    : Command(name, subsystem) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double period, Subsystem& subsystem)
+    : Command(name, subsystem) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period,
+                       Subsystem& subsystem) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+void PIDCommand::_Initialize() { m_controller->Enable(); }
+
+void PIDCommand::_End() { m_controller->Disable(); }
+
+void PIDCommand::_Interrupted() { _End(); }
+
+void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
+  SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+
+std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
+  return m_controller;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+  m_controller->SetSetpoint(setpoint);
+}
+
+double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+
+double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+
+void PIDCommand::InitSendable(SendableBuilder& builder) {
+  m_controller->InitSendable(builder);
+  Command::InitSendable(builder);
+  builder.SetSmartDashboardType("PIDCommand");
+}

wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp

@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDSubsystem.h"
+
+#include "frc/PIDController.h"
+
+using namespace frc;
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
+    : Subsystem(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+                           double f)
+    : Subsystem(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+                           double f, double period)
+    : Subsystem(name) {
+  m_controller =
+      std::make_shared<PIDController>(p, i, d, f, this, this, period);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+    : Subsystem("PIDSubsystem") {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+    : Subsystem("PIDSubsystem") {
+  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+                           double period)
+    : Subsystem("PIDSubsystem") {
+  m_controller =
+      std::make_shared<PIDController>(p, i, d, f, this, this, period);
+  AddChild("PIDController", m_controller);
+}
+
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetSetpoint(double setpoint) {
+  m_controller->SetSetpoint(setpoint);
+}
+
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+  SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
+  m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
+  m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
+  m_controller->SetAbsoluteTolerance(absValue);
+}
+
+void PIDSubsystem::SetPercentTolerance(double percent) {
+  m_controller->SetPercentTolerance(percent);
+}
+
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+  return m_controller;
+}

wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+    : InstantCommand("Print \"" + message + wpi::Twine('"')) {
+  m_message = message.str();
+}
+
+void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }

wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp

@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Scheduler.h"
+
+#include <algorithm>
+#include <set>
+#include <string>
+#include <vector>
+
+#include <hal/HAL.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/mutex.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/buttons/ButtonScheduler.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+struct Scheduler::Impl {
+  void Remove(Command* command);
+  void ProcessCommandAddition(Command* command);
+
+  using SubsystemSet = std::set<Subsystem*>;
+  SubsystemSet subsystems;
+  wpi::mutex buttonsMutex;
+  using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
+  ButtonVector buttons;
+  using CommandVector = std::vector<Command*>;
+  wpi::mutex additionsMutex;
+  CommandVector additions;
+  using CommandSet = std::set<Command*>;
+  CommandSet commands;
+  bool adding = false;
+  bool enabled = true;
+  std::vector<std::string> commandsBuf;
+  std::vector<double> idsBuf;
+  bool runningCommandsChanged = false;
+};
+
+Scheduler* Scheduler::GetInstance() {
+  static Scheduler instance;
+  return &instance;
+}
+
+void Scheduler::AddCommand(Command* command) {
+  std::scoped_lock lock(m_impl->additionsMutex);
+  if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
+      m_impl->additions.end())
+    return;
+  m_impl->additions.push_back(command);
+}
+
+void Scheduler::AddButton(ButtonScheduler* button) {
+  std::scoped_lock lock(m_impl->buttonsMutex);
+  m_impl->buttons.emplace_back(button);
+}
+
+void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
+  if (subsystem == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+    return;
+  }
+  m_impl->subsystems.insert(subsystem);
+}
+
+void Scheduler::Run() {
+  // Get button input (going backwards preserves button priority)
+  {
+    if (!m_impl->enabled) return;
+
+    std::scoped_lock lock(m_impl->buttonsMutex);
+    for (auto& button : m_impl->buttons) {
+      button->Execute();
+    }
+  }
+
+  // Call every subsystem's periodic method
+  for (auto& subsystem : m_impl->subsystems) {
+    subsystem->Periodic();
+  }
+
+  m_impl->runningCommandsChanged = false;
+
+  // Loop through the commands
+  for (auto cmdIter = m_impl->commands.begin();
+       cmdIter != m_impl->commands.end();) {
+    Command* command = *cmdIter;
+    // Increment before potentially removing to keep the iterator valid
+    ++cmdIter;
+    if (!command->Run()) {
+      Remove(command);
+      m_impl->runningCommandsChanged = true;
+    }
+  }
+
+  // Add the new things
+  {
+    std::scoped_lock lock(m_impl->additionsMutex);
+    for (auto& addition : m_impl->additions) {
+      // Check to make sure no adding during adding
+      if (m_impl->adding) {
+        wpi_setWPIErrorWithContext(IncompatibleState,
+                                   "Can not start command from cancel method");
+      } else {
+        m_impl->ProcessCommandAddition(addition);
+      }
+    }
+    m_impl->additions.clear();
+  }
+
+  // Add in the defaults
+  for (auto& subsystem : m_impl->subsystems) {
+    if (subsystem->GetCurrentCommand() == nullptr) {
+      if (m_impl->adding) {
+        wpi_setWPIErrorWithContext(IncompatibleState,
+                                   "Can not start command from cancel method");
+      } else {
+        m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
+      }
+    }
+    subsystem->ConfirmCommand();
+  }
+}
+
+void Scheduler::Remove(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+
+  m_impl->Remove(command);
+}
+
+void Scheduler::RemoveAll() {
+  while (m_impl->commands.size() > 0) {
+    Remove(*m_impl->commands.begin());
+  }
+}
+
+void Scheduler::ResetAll() {
+  RemoveAll();
+  m_impl->subsystems.clear();
+  m_impl->buttons.clear();
+  m_impl->additions.clear();
+  m_impl->commands.clear();
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
+
+void Scheduler::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Scheduler");
+  auto namesEntry = builder.GetEntry("Names");
+  auto idsEntry = builder.GetEntry("Ids");
+  auto cancelEntry = builder.GetEntry("Cancel");
+  builder.SetUpdateTable([=]() {
+    // Get the list of possible commands to cancel
+    auto new_toCancel = cancelEntry.GetValue();
+    wpi::ArrayRef<double> toCancel;
+    if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
+
+    // Cancel commands whose cancel buttons were pressed on the SmartDashboard
+    if (!toCancel.empty()) {
+      for (auto& command : m_impl->commands) {
+        for (const auto& cancelled : toCancel) {
+          if (command->GetID() == cancelled) {
+            command->Cancel();
+          }
+        }
+      }
+      nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
+    }
+
+    // Set the running commands
+    if (m_impl->runningCommandsChanged) {
+      m_impl->commandsBuf.resize(0);
+      m_impl->idsBuf.resize(0);
+      auto& registry = SendableRegistry::GetInstance();
+      for (const auto& command : m_impl->commands) {
+        m_impl->commandsBuf.emplace_back(registry.GetName(command));
+        m_impl->idsBuf.emplace_back(command->GetID());
+      }
+      nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
+      nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
+    }
+  });
+}
+
+Scheduler::Scheduler() : m_impl(new Impl) {
+  HAL_Report(HALUsageReporting::kResourceType_Command,
+             HALUsageReporting::kCommand_Scheduler);
+  SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+Scheduler::~Scheduler() {}
+
+void Scheduler::Impl::Remove(Command* command) {
+  if (!commands.erase(command)) return;
+
+  for (auto&& requirement : command->GetRequirements()) {
+    requirement->SetCurrentCommand(nullptr);
+  }
+
+  command->Removed();
+}
+
+void Scheduler::Impl::ProcessCommandAddition(Command* command) {
+  if (command == nullptr) return;
+
+  // Only add if not already in
+  auto found = commands.find(command);
+  if (found == commands.end()) {
+    // Check that the requirements can be had
+    const auto& requirements = command->GetRequirements();
+    for (const auto& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr &&
+          !requirement->GetCurrentCommand()->IsInterruptible())
+        return;
+    }
+
+    // Give it the requirements
+    adding = true;
+    for (auto&& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr) {
+        requirement->GetCurrentCommand()->Cancel();
+        Remove(requirement->GetCurrentCommand());
+      }
+      requirement->SetCurrentCommand(command);
+    }
+    adding = false;
+
+    commands.insert(command);
+
+    command->StartRunning();
+    runningCommandsChanged = true;
+  }
+}

wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/StartCommand.h"
+
+using namespace frc;
+
+StartCommand::StartCommand(Command* commandToStart)
+    : InstantCommand("StartCommand") {
+  m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }

wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp

@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Subsystem.h"
+
+#include "frc/WPIErrors.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Subsystem::Subsystem(const wpi::Twine& name) {
+  SendableRegistry::GetInstance().AddLW(this, name, name);
+  Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+
+void Subsystem::SetDefaultCommand(Command* command) {
+  if (command == nullptr) {
+    m_defaultCommand = nullptr;
+  } else {
+    const auto& reqs = command->GetRequirements();
+    if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
+      wpi_setWPIErrorWithContext(
+          CommandIllegalUse, "A default command must require the subsystem");
+      return;
+    }
+
+    m_defaultCommand = command;
+  }
+}
+
+Command* Subsystem::GetDefaultCommand() {
+  if (!m_initializedDefaultCommand) {
+    m_initializedDefaultCommand = true;
+    InitDefaultCommand();
+  }
+  return m_defaultCommand;
+}
+
+wpi::StringRef Subsystem::GetDefaultCommandName() {
+  Command* defaultCommand = GetDefaultCommand();
+  if (defaultCommand) {
+    return SendableRegistry::GetInstance().GetName(defaultCommand);
+  } else {
+    return wpi::StringRef();
+  }
+}
+
+void Subsystem::SetCurrentCommand(Command* command) {
+  m_currentCommand = command;
+  m_currentCommandChanged = true;
+}
+
+Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+wpi::StringRef Subsystem::GetCurrentCommandName() const {
+  Command* currentCommand = GetCurrentCommand();
+  if (currentCommand) {
+    return SendableRegistry::GetInstance().GetName(currentCommand);
+  } else {
+    return wpi::StringRef();
+  }
+}
+
+void Subsystem::Periodic() {}
+
+void Subsystem::InitDefaultCommand() {}
+
+std::string Subsystem::GetName() const {
+  return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Subsystem::SetName(const wpi::Twine& name) {
+  SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Subsystem::GetSubsystem() const {
+  return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Subsystem::SetSubsystem(const wpi::Twine& name) {
+  SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name,
+                         std::shared_ptr<Sendable> child) {
+  AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
+  AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
+  auto& registry = SendableRegistry::GetInstance();
+  registry.AddLW(&child, registry.GetSubsystem(this), name);
+  registry.AddChild(this, &child);
+}
+
+void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable& child) {
+  auto& registry = SendableRegistry::GetInstance();
+  registry.SetSubsystem(&child, registry.GetSubsystem(this));
+  registry.EnableLiveWindow(&child);
+  registry.AddChild(this, &child);
+}
+
+void Subsystem::ConfirmCommand() {
+  if (m_currentCommandChanged) m_currentCommandChanged = false;
+}
+
+void Subsystem::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+
+  builder.AddBooleanProperty(
+      ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
+  builder.AddStringProperty(".default",
+                            [=]() { return GetDefaultCommandName(); }, nullptr);
+
+  builder.AddBooleanProperty(
+      ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
+  builder.AddStringProperty(".command",
+                            [=]() { return GetCurrentCommandName(); }, nullptr);
+}

wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-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 "frc/commands/TimedCommand.h"
+
+using namespace frc;
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
+    : Command(name, timeout) {}
+
+TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
+                           Subsystem& subsystem)
+    : Command(name, timeout, subsystem) {}
+
+TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
+    : Command(timeout, subsystem) {}
+
+bool TimedCommand::IsFinished() { return IsTimedOut(); }

wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitCommand.h"
+
+using namespace frc;
+
+WaitCommand::WaitCommand(double timeout)
+    : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+
+WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
+    : TimedCommand(name, timeout) {}

wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitForChildren.h"
+
+#include "frc/commands/CommandGroup.h"
+
+using namespace frc;
+
+WaitForChildren::WaitForChildren(double timeout)
+    : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
+    : Command(name, timeout) {}
+
+bool WaitForChildren::IsFinished() {
+  return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}

wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitUntilCommand.h"
+
+#include "frc/Timer.h"
+
+using namespace frc;
+
+WaitUntilCommand::WaitUntilCommand(double time)
+    : Command("WaitUntilCommand", time) {
+  m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
+    : Command(name, time) {
+  m_time = time;
+}
+
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }

wpilibOldCommands/src/main/native/include/frc/buttons/Button.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/Trigger.h"
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * It is very easy to link a button to a command.  For instance, you could link
+ * the trigger button of a joystick to a "score" command.
+ *
+ * This class represents a subclass of Trigger that is specifically aimed at
+ * buttons on an operator interface as a common use case of the more generalized
+ * Trigger objects. This is a simple wrapper around Trigger with the method
+ * names renamed to fit the Button object use.
+ */
+class Button : public Trigger {
+ public:
+  Button() = default;
+  Button(Button&&) = default;
+  Button& operator=(Button&&) = default;
+
+  /**
+   * Specifies the command to run when a button is first pressed.
+   *
+   * @param command The pointer to the command to run
+   */
+  virtual void WhenPressed(Command* command);
+
+  /**
+   * Specifies the command to be scheduled while the button is pressed.
+   *
+   * The command will be scheduled repeatedly while the button is pressed and
+   * will be canceled when the button is released.
+   *
+   * @param command The pointer to the command to run
+   */
+  virtual void WhileHeld(Command* command);
+
+  /**
+   * Specifies the command to run when the button is released.
+   *
+   * The command will be scheduled a single time.
+   *
+   * @param command The pointer to the command to run
+   */
+  virtual void WhenReleased(Command* command);
+
+  /**
+   * Cancels the specificed command when the button is pressed.
+   *
+   * @param command The command to be canceled
+   */
+  virtual void CancelWhenPressed(Command* command);
+
+  /**
+   * Toggle the specified command when the button is pressed.
+   *
+   * @param command The command to be toggled
+   */
+  virtual void ToggleWhenPressed(Command* command);
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ButtonScheduler {
+ public:
+  ButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~ButtonScheduler() = default;
+
+  ButtonScheduler(ButtonScheduler&&) = default;
+  ButtonScheduler& operator=(ButtonScheduler&&) = default;
+
+  virtual void Execute() = 0;
+  void Start();
+
+ protected:
+  bool m_pressedLast;
+  Trigger* m_button;
+  Command* m_command;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class CancelButtonScheduler : public ButtonScheduler {
+ public:
+  CancelButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~CancelButtonScheduler() = default;
+
+  CancelButtonScheduler(CancelButtonScheduler&&) = default;
+  CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
+
+  virtual void Execute();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class HeldButtonScheduler : public ButtonScheduler {
+ public:
+  HeldButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~HeldButtonScheduler() = default;
+
+  HeldButtonScheduler(HeldButtonScheduler&&) = default;
+  HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
+
+  virtual void Execute();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class InternalButton : public Button {
+ public:
+  InternalButton() = default;
+  explicit InternalButton(bool inverted);
+  virtual ~InternalButton() = default;
+
+  InternalButton(InternalButton&&) = default;
+  InternalButton& operator=(InternalButton&&) = default;
+
+  void SetInverted(bool inverted);
+  void SetPressed(bool pressed);
+
+  virtual bool Get();
+
+ private:
+  bool m_pressed = false;
+  bool m_inverted = false;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/GenericHID.h"
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class JoystickButton : public Button {
+ public:
+  JoystickButton(GenericHID* joystick, int buttonNumber);
+  virtual ~JoystickButton() = default;
+
+  JoystickButton(JoystickButton&&) = default;
+  JoystickButton& operator=(JoystickButton&&) = default;
+
+  virtual bool Get();
+
+ private:
+  GenericHID* m_joystick;
+  int m_buttonNumber;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/Twine.h>
+
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class NetworkButton : public Button {
+ public:
+  NetworkButton(const wpi::Twine& tableName, const wpi::Twine& field);
+  NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                const wpi::Twine& field);
+  virtual ~NetworkButton() = default;
+
+  NetworkButton(NetworkButton&&) = default;
+  NetworkButton& operator=(NetworkButton&&) = default;
+
+  virtual bool Get();
+
+ private:
+  nt::NetworkTableEntry m_entry;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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 "frc/buttons/Button.h"
+
+namespace frc {
+class POVButton : public Button {
+ public:
+  /**
+   * Creates a POV button for triggering commands.
+   *
+   * @param joystick The GenericHID object that has the POV
+   * @param angle The desired angle in degrees (e.g. 90, 270)
+   * @param povNumber The POV number (@see GenericHID#GetPOV)
+   */
+  POVButton(GenericHID& joystick, int angle, int povNumber = 0);
+  virtual ~POVButton() = default;
+
+  POVButton(POVButton&&) = default;
+  POVButton& operator=(POVButton&&) = default;
+
+  bool Get() override;
+
+ private:
+  GenericHID* m_joystick;
+  int m_angle;
+  int m_povNumber;
+};
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class PressedButtonScheduler : public ButtonScheduler {
+ public:
+  PressedButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~PressedButtonScheduler() = default;
+
+  PressedButtonScheduler(PressedButtonScheduler&&) = default;
+  PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
+
+  virtual void Execute();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ReleasedButtonScheduler : public ButtonScheduler {
+ public:
+  ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~ReleasedButtonScheduler() = default;
+
+  ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
+  ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
+
+  virtual void Execute();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ToggleButtonScheduler : public ButtonScheduler {
+ public:
+  ToggleButtonScheduler(bool last, Trigger* button, Command* orders);
+  virtual ~ToggleButtonScheduler() = default;
+
+  ToggleButtonScheduler(ToggleButtonScheduler&&) = default;
+  ToggleButtonScheduler& operator=(ToggleButtonScheduler&&) = default;
+
+  virtual void Execute();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class Command;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * It is very easy to link a polled input to a command. For instance, you could
+ * link the trigger button of a joystick to a "score" command or an encoder
+ * reaching a particular value.
+ *
+ * It is encouraged that teams write a subclass of Trigger if they want to have
+ * something unusual (for instance, if they want to react to the user holding
+ * a button while the robot is reading a certain sensor input). For this, they
+ * only have to write the {@link Trigger#Get()} method to get the full
+ * functionality of the Trigger class.
+ */
+class Trigger : public Sendable, public SendableHelper<Trigger> {
+ public:
+  Trigger() = default;
+  ~Trigger() override = default;
+
+  Trigger(const Trigger& rhs);
+  Trigger& operator=(const Trigger& rhs);
+  Trigger(Trigger&& rhs);
+  Trigger& operator=(Trigger&& rhs);
+
+  bool Grab();
+  virtual bool Get() = 0;
+  void WhenActive(Command* command);
+  void WhileActive(Command* command);
+  void WhenInactive(Command* command);
+  void CancelWhenActive(Command* command);
+  void ToggleWhenActive(Command* command);
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  std::atomic_bool m_sendablePressed{false};
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/Command.h

@@ -0,0 +1,492 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/SmallPtrSet.h>
+#include <wpi/Twine.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class CommandGroup;
+
+/**
+ * The Command class is at the very core of the entire command framework.
+ *
+ * Every command can be started with a call to Start(). Once a command is
+ * started it will call Initialize(), and then will repeatedly call Execute()
+ * until the IsFinished() returns true. Once it does,End() will be called.
+ *
+ * However, if at any point while it is running Cancel() is called, then the
+ * command will be stopped and Interrupted() will be called.
+ *
+ * If a command uses a Subsystem, then it should specify that it does so by
+ * calling the Requires() method in its constructor. Note that a Command may
+ * have multiple requirements, and Requires() should be called for each one.
+ *
+ * If a command is running and a new command with shared requirements is
+ * started, then one of two things will happen. If the active command is
+ * interruptible, then Cancel() will be called and the command will be removed
+ * to make way for the new one. If the active command is not interruptible, the
+ * other one will not even be started, and the active one will continue
+ * functioning.
+ *
+ * @see CommandGroup
+ * @see Subsystem
+ */
+class Command : public ErrorBase,
+                public Sendable,
+                public SendableHelper<Command> {
+  friend class CommandGroup;
+  friend class Scheduler;
+
+ public:
+  /**
+   * Creates a new command.
+   *
+   * The name of this command will be default.
+   */
+  Command();
+
+  /**
+   * Creates a new command with the given name and no timeout.
+   *
+   * @param name the name for this command
+   */
+  explicit Command(const wpi::Twine& name);
+
+  /**
+   * Creates a new command with the given timeout and a default name.
+   *
+   * @param timeout the time (in seconds) before this command "times out"
+   * @see IsTimedOut()
+   */
+  explicit Command(double timeout);
+
+  /**
+   * Creates a new command with the given timeout and a default name.
+   *
+   * @param subsystem the subsystem that the command requires
+   */
+  explicit Command(Subsystem& subsystem);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name    the name of the command
+   * @param timeout the time (in seconds) before this command "times out"
+   * @see IsTimedOut()
+   */
+  Command(const wpi::Twine& name, double timeout);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name      the name of the command
+   * @param subsystem the subsystem that the command requires
+   */
+  Command(const wpi::Twine& name, Subsystem& subsystem);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param timeout   the time (in seconds) before this command "times out"
+   * @param subsystem the subsystem that the command requires @see IsTimedOut()
+   */
+  Command(double timeout, Subsystem& subsystem);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name      the name of the command
+   * @param timeout   the time (in seconds) before this command "times out"
+   * @param subsystem the subsystem that the command requires @see IsTimedOut()
+   */
+  Command(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+
+  ~Command() override = default;
+
+  Command(Command&&) = default;
+  Command& operator=(Command&&) = default;
+
+  /**
+   * Returns the time since this command was initialized (in seconds).
+   *
+   * This function will work even if there is no specified timeout.
+   *
+   * @return the time since this command was initialized (in seconds).
+   */
+  double TimeSinceInitialized() const;
+
+  /**
+   * This method specifies that the given Subsystem is used by this command.
+   *
+   * This method is crucial to the functioning of the Command System in general.
+   *
+   * Note that the recommended way to call this method is in the constructor.
+   *
+   * @param subsystem The Subsystem required
+   * @see Subsystem
+   */
+  void Requires(Subsystem* s);
+
+  /**
+   * Starts up the command. Gets the command ready to start.
+   *
+   * Note that the command will eventually start, however it will not
+   * necessarily do so immediately, and may in fact be canceled before
+   * initialize is even called.
+   */
+  void Start();
+
+  /**
+   * The run method is used internally to actually run the commands.
+   *
+   * @return Whether or not the command should stay within the Scheduler.
+   */
+  bool Run();
+
+  /**
+   * This will cancel the current command.
+   *
+   * This will cancel the current command eventually. It can be called multiple
+   * times. And it can be called when the command is not running. If the command
+   * is running though, then the command will be marked as canceled and
+   * eventually removed.
+   *
+   * A command can not be canceled if it is a part of a command group, you must
+   * cancel the command group instead.
+   */
+  void Cancel();
+
+  /**
+   * Returns whether or not the command is running.
+   *
+   * This may return true even if the command has just been canceled, as it may
+   * not have yet called Interrupted().
+   *
+   * @return whether or not the command is running
+   */
+  bool IsRunning() const;
+
+  /**
+   * Returns whether or not the command has been initialized.
+   *
+   * @return whether or not the command has been initialized.
+   */
+  bool IsInitialized() const;
+
+  /**
+   * Returns whether or not the command has completed running.
+   *
+   * @return whether or not the command has completed running.
+   */
+  bool IsCompleted() const;
+
+  /**
+   * Returns whether or not this has been canceled.
+   *
+   * @return whether or not this has been canceled
+   */
+  bool IsCanceled() const;
+
+  /**
+   * Returns whether or not this command can be interrupted.
+   *
+   * @return whether or not this command can be interrupted
+   */
+  bool IsInterruptible() const;
+
+  /**
+   * Sets whether or not this command can be interrupted.
+   *
+   * @param interruptible whether or not this command can be interrupted
+   */
+  void SetInterruptible(bool interruptible);
+
+  /**
+   * Checks if the command requires the given Subsystem.
+   *
+   * @param system the system
+   * @return whether or not the subsystem is required (false if given nullptr)
+   */
+  bool DoesRequire(Subsystem* subsystem) const;
+
+  using SubsystemSet = wpi::SmallPtrSetImpl<Subsystem*>;
+
+  /**
+   * Returns the requirements (as an std::set of Subsystem pointers) of this
+   * command.
+   *
+   * @return The requirements (as an std::set of Subsystem pointers) of this
+   *         command
+   */
+  const SubsystemSet& GetRequirements() const;
+
+  /**
+   * Returns the CommandGroup that this command is a part of.
+   *
+   * Will return null if this Command is not in a group.
+   *
+   * @return The CommandGroup that this command is a part of (or null if not in
+   *         group)
+   */
+  CommandGroup* GetGroup() const;
+
+  /**
+   * Sets whether or not this Command should run when the robot is disabled.
+   *
+   * By default a command will not run when the robot is disabled, and will in
+   * fact be canceled.
+   *
+   * @param run Whether this command should run when the robot is disabled.
+   */
+  void SetRunWhenDisabled(bool run);
+
+  /**
+   * Returns whether or not this Command will run when the robot is disabled, or
+   * if it will cancel itself.
+   *
+   * @return Whether this Command will run when the robot is disabled, or if it
+   * will cancel itself.
+   */
+  bool WillRunWhenDisabled() const;
+
+  /**
+   * Get the ID (sequence number) for this command.
+   *
+   * The ID is a unique sequence number that is incremented for each command.
+   *
+   * @return The ID of this command
+   */
+  int GetID() const;
+
+ protected:
+  /**
+   * Sets the timeout of this command.
+   *
+   * @param timeout the timeout (in seconds)
+   * @see IsTimedOut()
+   */
+  void SetTimeout(double timeout);
+
+  /**
+   * Returns whether or not the TimeSinceInitialized() method returns a number
+   * which is greater than or equal to the timeout for the command.
+   *
+   * If there is no timeout, this will always return false.
+   *
+   * @return whether the time has expired
+   */
+  bool IsTimedOut() const;
+
+  /**
+   * If changes are locked, then this will generate a CommandIllegalUse error.
+   *
+   * @param message The message to report on error (it is appended by a default
+   *                message)
+   * @return True if assert passed, false if assert failed.
+   */
+  bool AssertUnlocked(const std::string& message);
+
+  /**
+   * Sets the parent of this command. No actual change is made to the group.
+   *
+   * @param parent the parent
+   */
+  void SetParent(CommandGroup* parent);
+
+  /**
+   * Returns whether the command has a parent.
+   *
+   * @param True if the command has a parent.
+   */
+  bool IsParented() const;
+
+  /**
+   * Clears list of subsystem requirements.
+   *
+   * This is only used by ConditionalCommand so cancelling the chosen command
+   * works properly in CommandGroup.
+   */
+  void ClearRequirements();
+
+  /**
+   * The initialize method is called the first time this Command is run after
+   * being started.
+   */
+  virtual void Initialize();
+
+  /**
+   * The execute method is called repeatedly until this Command either finishes
+   * or is canceled.
+   */
+  virtual void Execute();
+
+  /**
+   * Returns whether this command is finished.
+   *
+   * If it is, then the command will be removed and End() will be called.
+   *
+   * It may be useful for a team to reference the IsTimedOut() method for
+   * time-sensitive commands.
+   *
+   * Returning false will result in the command never ending automatically.
+   * It may still be cancelled manually or interrupted by another command.
+   * Returning true will result in the command executing once and finishing
+   * immediately. We recommend using InstantCommand for this.
+   *
+   * @return Whether this command is finished.
+   * @see IsTimedOut()
+   */
+  virtual bool IsFinished() = 0;
+
+  /**
+   * Called when the command ended peacefully.
+   *
+   * This is where you may want to wrap up loose ends, like shutting off a motor
+   * that was being used in the command.
+   */
+  virtual void End();
+
+  /**
+   * Called when the command ends because somebody called Cancel() or another
+   * command shared the same requirements as this one, and booted it out.
+   *
+   * This is where you may want to wrap up loose ends, like shutting off a motor
+   * that was being used in the command.
+   *
+   * Generally, it is useful to simply call the End() method within this method,
+   * as done here.
+   */
+  virtual void Interrupted();
+
+  virtual void _Initialize();
+  virtual void _Interrupted();
+  virtual void _Execute();
+  virtual void _End();
+
+  /**
+   * This works like Cancel(), except that it doesn't throw an exception if it
+   * is a part of a command group.
+   *
+   * Should only be called by the parent command group.
+   */
+  virtual void _Cancel();
+
+  friend class ConditionalCommand;
+
+  /**
+   * Gets the name of this Command.
+   *
+   * @return Name
+   */
+  std::string GetName() const;
+
+  /**
+   * Sets the name of this Command.
+   *
+   * @param name name
+   */
+  void SetName(const wpi::Twine& name);
+
+  /**
+   * Gets the subsystem name of this Command.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Command.
+   *
+   * @param subsystem subsystem name
+   */
+  void SetSubsystem(const wpi::Twine& subsystem);
+
+ private:
+  /**
+   * Prevents further changes from being made.
+   */
+  void LockChanges();
+
+  /**
+   * Called when the command has been removed.
+   *
+   * This will call Interrupted() or End().
+   */
+  void Removed();
+
+  /**
+   * This is used internally to mark that the command has been started.
+   *
+   * The lifecycle of a command is:
+   *
+   * StartRunning() is called. Run() is called (multiple times potentially).
+   * Removed() is called.
+   *
+   * It is very important that StartRunning() and Removed() be called in order
+   * or some assumptions of the code will be broken.
+   */
+  void StartRunning();
+
+  /**
+   * Called to indicate that the timer should start.
+   *
+   * This is called right before Initialize() is, inside the Run() method.
+   */
+  void StartTiming();
+
+  // The time since this command was initialized
+  double m_startTime = -1;
+
+  // The time (in seconds) before this command "times out" (-1 if no timeout)
+  double m_timeout;
+
+  // Whether or not this command has been initialized
+  bool m_initialized = false;
+
+  // The requirements (or null if no requirements)
+  wpi::SmallPtrSet<Subsystem*, 4> m_requirements;
+
+  // Whether or not it is running
+  bool m_running = false;
+
+  // Whether or not it is interruptible
+  bool m_interruptible = true;
+
+  // Whether or not it has been canceled
+  bool m_canceled = false;
+
+  // Whether or not it has been locked
+  bool m_locked = false;
+
+  // Whether this command should run when the robot is disabled
+  bool m_runWhenDisabled = false;
+
+  // The CommandGroup this is in
+  CommandGroup* m_parent = nullptr;
+
+  // Whether or not this command has completed running
+  bool m_completed = false;
+
+  int m_commandID = m_commandCounter++;
+  static int m_commandCounter;
+
+ public:
+  void InitSendable(SendableBuilder& builder) override;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h

@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+#include "frc/commands/CommandGroupEntry.h"
+
+namespace frc {
+
+/**
+ * A CommandGroup is a list of commands which are executed in sequence.
+ *
+ * Commands in a CommandGroup are added using the AddSequential() method and are
+ * called sequentially. CommandGroups are themselves Commands and can be given
+ * to other CommandGroups.
+ *
+ * CommandGroups will carry all of the requirements of their Command
+ * subcommands. Additional requirements can be specified by calling Requires()
+ * normally in the constructor.
+ *
+ * CommandGroups can also execute commands in parallel, simply by adding them
+ * using AddParallel().
+ *
+ * @see Command
+ * @see Subsystem
+ */
+class CommandGroup : public Command {
+ public:
+  CommandGroup() = default;
+
+  /**
+   * Creates a new CommandGroup with the given name.
+   *
+   * @param name The name for this command group
+   */
+  explicit CommandGroup(const wpi::Twine& name);
+
+  virtual ~CommandGroup() = default;
+
+  CommandGroup(CommandGroup&&) = default;
+  CommandGroup& operator=(CommandGroup&&) = default;
+
+  /**
+   * Adds a new Command to the group. The Command will be started after all the
+   * previously added Commands.
+   *
+   * Note that any requirements the given Command has will be added to the
+   * group. For this reason, a Command's requirements can not be changed after
+   * being added to a group.
+   *
+   * It is recommended that this method be called in the constructor.
+   *
+   * @param command The Command to be added
+   */
+  void AddSequential(Command* command);
+
+  /**
+   * Adds a new Command to the group with a given timeout. The Command will be
+   * started after all the previously added commands.
+   *
+   * Once the Command is started, it will be run until it finishes or the time
+   * expires, whichever is sooner.  Note that the given Command will have no
+   * knowledge that it is on a timer.
+   *
+   * Note that any requirements the given Command has will be added to the
+   * group. For this reason, a Command's requirements can not be changed after
+   * being added to a group.
+   *
+   * It is recommended that this method be called in the constructor.
+   *
+   * @param command The Command to be added
+   * @param timeout The timeout (in seconds)
+   */
+  void AddSequential(Command* command, double timeout);
+
+  /**
+   * Adds a new child Command to the group. The Command will be started after
+   * all the previously added Commands.
+   *
+   * Instead of waiting for the child to finish, a CommandGroup will have it run
+   * at the same time as the subsequent Commands. The child will run until
+   * either it finishes, a new child with conflicting requirements is started,
+   * or the main sequence runs a Command with conflicting requirements. In the
+   * latter two cases, the child will be canceled even if it says it can't be
+   * interrupted.
+   *
+   * Note that any requirements the given Command has will be added to the
+   * group. For this reason, a Command's requirements can not be changed after
+   * being added to a group.
+   *
+   * It is recommended that this method be called in the constructor.
+   *
+   * @param command The command to be added
+   */
+  void AddParallel(Command* command);
+
+  /**
+   * Adds a new child Command to the group with the given timeout. The Command
+   * will be started after all the previously added Commands.
+   *
+   * Once the Command is started, it will run until it finishes, is interrupted,
+   * or the time expires, whichever is sooner. Note that the given Command will
+   * have no knowledge that it is on a timer.
+   *
+   * Instead of waiting for the child to finish, a CommandGroup will have it run
+   * at the same time as the subsequent Commands. The child will run until
+   * either it finishes, the timeout expires, a new child with conflicting
+   * requirements is started, or the main sequence runs a Command with
+   * conflicting requirements. In the latter two cases, the child will be
+   * canceled even if it says it can't be interrupted.
+   *
+   * Note that any requirements the given Command has will be added to the
+   * group. For this reason, a Command's requirements can not be changed after
+   * being added to a group.
+   *
+   * It is recommended that this method be called in the constructor.
+   *
+   * @param command The command to be added
+   * @param timeout The timeout (in seconds)
+   */
+  void AddParallel(Command* command, double timeout);
+
+  bool IsInterruptible() const;
+
+  int GetSize() const;
+
+ protected:
+  /**
+   * Can be overridden by teams.
+   */
+  virtual void Initialize();
+
+  /**
+   * Can be overridden by teams.
+   */
+  virtual void Execute();
+
+  /**
+   * Can be overridden by teams.
+   */
+  virtual bool IsFinished();
+
+  /**
+   * Can be overridden by teams.
+   */
+  virtual void End();
+
+  /**
+   * Can be overridden by teams.
+   */
+  virtual void Interrupted();
+
+  virtual void _Initialize();
+  virtual void _Execute();
+  virtual void _End();
+  virtual void _Interrupted();
+
+ private:
+  void CancelConflicts(Command* command);
+
+  // The commands in this group (stored in entries)
+  std::vector<CommandGroupEntry> m_commands;
+
+  // The active children in this group (stored in entries)
+  std::vector<CommandGroupEntry*> m_children;
+
+  // The current command, -1 signifies that none have been run
+  int m_currentCommandIndex = -1;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+class Command;
+
+class CommandGroupEntry {
+ public:
+  enum Sequence {
+    kSequence_InSequence,
+    kSequence_BranchPeer,
+    kSequence_BranchChild
+  };
+
+  CommandGroupEntry() = default;
+  CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0);
+
+  CommandGroupEntry(CommandGroupEntry&&) = default;
+  CommandGroupEntry& operator=(CommandGroupEntry&&) = default;
+
+  bool IsTimedOut() const;
+
+  double m_timeout = -1.0;
+  Command* m_command = nullptr;
+  Sequence m_state = kSequence_InSequence;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h

@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * A ConditionalCommand is a Command that starts one of two commands.
+ *
+ * A ConditionalCommand uses the Condition method to determine whether it should
+ * run onTrue or onFalse.
+ *
+ * A ConditionalCommand adds the proper Command to the Scheduler during
+ * Initialize() and then IsFinished() will return true once that Command has
+ * finished executing.
+ *
+ * If no Command is specified for onFalse, the occurrence of that condition
+ * will be a no-op.
+ *
+ * A ConditionalCommand will require the superset of subsystems of the onTrue
+ * and onFalse commands.
+ *
+ * @see Command
+ * @see Scheduler
+ */
+class ConditionalCommand : public Command {
+ public:
+  /**
+   * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+   *
+   * @param onTrue  The Command to execute if Condition() returns true
+   * @param onFalse The Command to execute if Condition() returns false
+   */
+  explicit ConditionalCommand(Command* onTrue, Command* onFalse = nullptr);
+
+  /**
+   * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+   *
+   * @param name    The name for this command group
+   * @param onTrue  The Command to execute if Condition() returns true
+   * @param onFalse The Command to execute if Condition() returns false
+   */
+  ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+                     Command* onFalse = nullptr);
+
+  virtual ~ConditionalCommand() = default;
+
+  ConditionalCommand(ConditionalCommand&&) = default;
+  ConditionalCommand& operator=(ConditionalCommand&&) = default;
+
+ protected:
+  /**
+   * The Condition to test to determine which Command to run.
+   *
+   * @return true if m_onTrue should be run or false if m_onFalse should be run.
+   */
+  virtual bool Condition() = 0;
+
+  void _Initialize() override;
+  void _Cancel() override;
+  bool IsFinished() override;
+  void _Interrupted() override;
+
+ private:
+  // The Command to execute if Condition() returns true
+  Command* m_onTrue;
+
+  // The Command to execute if Condition() returns false
+  Command* m_onFalse;
+
+  // Stores command chosen by condition
+  Command* m_chosenCommand = nullptr;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-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 <functional>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+#include "frc/commands/Subsystem.h"
+
+namespace frc {
+
+/**
+ * This command will execute once, then finish immediately afterward.
+ *
+ * Subclassing InstantCommand is shorthand for returning true from IsFinished().
+ */
+class InstantCommand : public Command {
+ public:
+  /**
+   * Creates a new InstantCommand with the given name.
+   *
+   * @param name The name for this command
+   */
+  explicit InstantCommand(const wpi::Twine& name);
+
+  /**
+   * Creates a new InstantCommand with the given requirement.
+   *
+   * @param subsystem The subsystem that the command requires
+   */
+  explicit InstantCommand(Subsystem& subsystem);
+
+  /**
+   * Creates a new InstantCommand with the given name.
+   *
+   * @param name      The name for this command
+   * @param subsystem The subsystem that the command requires
+   */
+  InstantCommand(const wpi::Twine& name, Subsystem& subsystem);
+
+  /**
+   * Create a command that calls the given function when run.
+   *
+   * @param func The function to run when Initialize() is run.
+   */
+  explicit InstantCommand(std::function<void()> func);
+
+  /**
+   * Create a command that calls the given function when run.
+   *
+   * @param subsystem The subsystems that this command runs on.
+   * @param func      The function to run when Initialize() is run.
+   */
+  InstantCommand(Subsystem& subsystem, std::function<void()> func);
+
+  /**
+   * Create a command that calls the given function when run.
+   *
+   * @param name   The name of the command.
+   * @param func   The function to run when Initialize() is run.
+   */
+  InstantCommand(const wpi::Twine& name, std::function<void()> func);
+
+  /**
+   * Create a command that calls the given function when run.
+   *
+   * @param name      The name of the command.
+   * @param subsystem The subsystems that this command runs on.
+   * @param func      The function to run when Initialize() is run.
+   */
+  InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+                 std::function<void()> func);
+
+  InstantCommand() = default;
+  virtual ~InstantCommand() = default;
+
+  InstantCommand(InstantCommand&&) = default;
+  InstantCommand& operator=(InstantCommand&&) = default;
+
+ protected:
+  std::function<void()> m_func = nullptr;
+  void _Initialize() override;
+  bool IsFinished() override;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+#include "frc/PIDController.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class PIDCommand : public Command, public PIDOutput, public PIDSource {
+ public:
+  PIDCommand(const wpi::Twine& name, double p, double i, double d);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+             double period);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+             double period);
+  PIDCommand(double p, double i, double d);
+  PIDCommand(double p, double i, double d, double period);
+  PIDCommand(double p, double i, double d, double f, double period);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+             Subsystem& subsystem);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+             double period, Subsystem& subsystem);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+             double period, Subsystem& subsystem);
+  PIDCommand(double p, double i, double d, Subsystem& subsystem);
+  PIDCommand(double p, double i, double d, double period, Subsystem& subsystem);
+  PIDCommand(double p, double i, double d, double f, double period,
+             Subsystem& subsystem);
+  virtual ~PIDCommand() = default;
+
+  PIDCommand(PIDCommand&&) = default;
+  PIDCommand& operator=(PIDCommand&&) = default;
+
+  void SetSetpointRelative(double deltaSetpoint);
+
+  // PIDOutput interface
+  void PIDWrite(double output) override;
+
+  // PIDSource interface
+  double PIDGet() override;
+
+ protected:
+  std::shared_ptr<PIDController> GetPIDController() const;
+  void _Initialize() override;
+  void _Interrupted() override;
+  void _End() override;
+  void SetSetpoint(double setpoint);
+  double GetSetpoint() const;
+  double GetPosition();
+
+  virtual double ReturnPIDInput() = 0;
+  virtual void UsePIDOutput(double output) = 0;
+
+ private:
+  // The internal PIDController
+  std::shared_ptr<PIDController> m_controller;
+
+ public:
+  void InitSendable(SendableBuilder& builder) override;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h

@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+#include "frc/PIDController.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/commands/Subsystem.h"
+
+namespace frc {
+
+/**
+ * This class is designed to handle the case where there is a Subsystem which
+ * uses a single PIDController almost constantly (for instance, an elevator
+ * which attempts to stay at a constant height).
+ *
+ * It provides some convenience methods to run an internal PIDController. It
+ * also allows access to the internal PIDController in order to give total
+ * control to the programmer.
+ */
+class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
+ public:
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * @param name the name
+   * @param p    the proportional value
+   * @param i    the integral value
+   * @param d    the derivative value
+   */
+  PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
+
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * @param name the name
+   * @param p    the proportional value
+   * @param i    the integral value
+   * @param d    the derivative value
+   * @param f    the feedforward value
+   */
+  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
+
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * It will also space the time between PID loop calculations to be equal to
+   * the given period.
+   *
+   * @param name   the name
+   * @param p      the proportional value
+   * @param i      the integral value
+   * @param d      the derivative value
+   * @param f      the feedfoward value
+   * @param period the time (in seconds) between calculations
+   */
+  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f,
+               double period);
+
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * It will use the class name as its name.
+   *
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   */
+  PIDSubsystem(double p, double i, double d);
+
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * It will use the class name as its name.
+   *
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param f the feedforward value
+   */
+  PIDSubsystem(double p, double i, double d, double f);
+
+  /**
+   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   *
+   * It will use the class name as its name. It will also space the time
+   * between PID loop calculations to be equal to the given period.
+   *
+   * @param p      the proportional value
+   * @param i      the integral value
+   * @param d      the derivative value
+   * @param f      the feedforward value
+   * @param period the time (in seconds) between calculations
+   */
+  PIDSubsystem(double p, double i, double d, double f, double period);
+
+  ~PIDSubsystem() override = default;
+
+  PIDSubsystem(PIDSubsystem&&) = default;
+  PIDSubsystem& operator=(PIDSubsystem&&) = default;
+
+  /**
+   * Enables the internal PIDController.
+   */
+  void Enable();
+
+  /**
+   * Disables the internal PIDController.
+   */
+  void Disable();
+
+  // PIDOutput interface
+  void PIDWrite(double output) override;
+
+  // PIDSource interface
+
+  double PIDGet() override;
+
+  /**
+   * Sets the setpoint to the given value.
+   *
+   * If SetRange() was called, then the given setpoint will be trimmed to fit
+   * within the range.
+   *
+   * @param setpoint the new setpoint
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Adds the given value to the setpoint.
+   *
+   * If SetRange() was used, then the bounds will still be honored by this
+   * method.
+   *
+   * @param deltaSetpoint the change in the setpoint
+   */
+  void SetSetpointRelative(double deltaSetpoint);
+
+  /**
+   * Sets the maximum and minimum values expected from the input.
+   *
+   * @param minimumInput the minimum value expected from the input
+   * @param maximumInput the maximum value expected from the output
+   */
+  void SetInputRange(double minimumInput, double maximumInput);
+
+  /**
+   * Sets the maximum and minimum values to write.
+   *
+   * @param minimumOutput the minimum value to write to the output
+   * @param maximumOutput the maximum value to write to the output
+   */
+  void SetOutputRange(double minimumOutput, double maximumOutput);
+
+  /**
+   * Return the current setpoint.
+   *
+   * @return The current setpoint
+   */
+  double GetSetpoint();
+
+  /**
+   * Returns the current position.
+   *
+   * @return the current position
+   */
+  double GetPosition();
+
+  /**
+   * Returns the current rate.
+   *
+   * @return the current rate
+   */
+  double GetRate();
+
+  /**
+   * Set the absolute error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param absValue absolute error which is tolerable
+   */
+  virtual void SetAbsoluteTolerance(double absValue);
+
+  /**
+   * Set the percentage error which is considered tolerable for use with
+   * OnTarget().
+   *
+   * @param percent percentage error which is tolerable
+   */
+  virtual void SetPercentTolerance(double percent);
+
+  /**
+   * Return true if the error is within the percentage of the total input range,
+   * determined by SetTolerance().
+   *
+   * This asssumes that the maximum and minimum input were set using SetInput().
+   * Use OnTarget() in the IsFinished() method of commands that use this
+   * subsystem.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * @return True if the error is within the percentage tolerance of the input
+   *         range
+   */
+  virtual bool OnTarget() const;
+
+ protected:
+  /**
+   * Returns the PIDController used by this PIDSubsystem.
+   *
+   * Use this if you would like to fine tune the PID loop.
+   *
+   * @return The PIDController used by this PIDSubsystem
+   */
+  std::shared_ptr<PIDController> GetPIDController();
+
+  virtual double ReturnPIDInput() = 0;
+  virtual void UsePIDOutput(double output) = 0;
+
+ private:
+  // The internal PIDController
+  std::shared_ptr<PIDController> m_controller;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/InstantCommand.h"
+
+namespace frc {
+
+class PrintCommand : public InstantCommand {
+ public:
+  explicit PrintCommand(const wpi::Twine& message);
+  virtual ~PrintCommand() = default;
+
+  PrintCommand(PrintCommand&&) = default;
+  PrintCommand& operator=(PrintCommand&&) = default;
+
+ protected:
+  virtual void Initialize();
+
+ private:
+  std::string m_message;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class ButtonScheduler;
+class Command;
+class Subsystem;
+
+class Scheduler : public ErrorBase,
+                  public Sendable,
+                  public SendableHelper<Scheduler> {
+ public:
+  /**
+   * Returns the Scheduler, creating it if one does not exist.
+   *
+   * @return the Scheduler
+   */
+  static Scheduler* GetInstance();
+
+  /**
+   * Add a command to be scheduled later.
+   *
+   * In any pass through the scheduler, all commands are added to the additions
+   * list, then at the end of the pass, they are all scheduled.
+   *
+   * @param command The command to be scheduled
+   */
+  void AddCommand(Command* command);
+
+  void AddButton(ButtonScheduler* button);
+
+  /**
+   * Registers a Subsystem to this Scheduler, so that the Scheduler might know
+   * if a default Command needs to be run.
+   *
+   * All Subsystems should call this.
+   *
+   * @param system the system
+   */
+  void RegisterSubsystem(Subsystem* subsystem);
+
+  /**
+   * Runs a single iteration of the loop.
+   *
+   * This method should be called often in order to have a functioning
+   * Command system. The loop has five stages:
+   *
+   * <ol>
+   *   <li>Poll the Buttons</li>
+   *   <li>Execute/Remove the Commands</li>
+   *   <li>Send values to SmartDashboard</li>
+   *   <li>Add Commands</li>
+   *   <li>Add Defaults</li>
+   * </ol>
+   */
+  void Run();
+
+  /**
+   * Removes the Command from the Scheduler.
+   *
+   * @param command the command to remove
+   */
+  void Remove(Command* command);
+
+  void RemoveAll();
+
+  /**
+   * Completely resets the scheduler. Undefined behavior if running.
+   */
+  void ResetAll();
+
+  void SetEnabled(bool enabled);
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  Scheduler();
+  ~Scheduler() override;
+
+  Scheduler(Scheduler&&) = default;
+  Scheduler& operator=(Scheduler&&) = default;
+
+  struct Impl;
+  std::unique_ptr<Impl> m_impl;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/commands/InstantCommand.h"
+
+namespace frc {
+
+class StartCommand : public InstantCommand {
+ public:
+  explicit StartCommand(Command* commandToStart);
+  virtual ~StartCommand() = default;
+
+  StartCommand(StartCommand&&) = default;
+  StartCommand& operator=(StartCommand&&) = default;
+
+ protected:
+  virtual void Initialize();
+
+ private:
+  Command* m_commandToFork;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h

@@ -0,0 +1,199 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class Command;
+
+class Subsystem : public ErrorBase,
+                  public Sendable,
+                  public SendableHelper<Subsystem> {
+  friend class Scheduler;
+
+ public:
+  /**
+   * Creates a subsystem with the given name.
+   *
+   * @param name the name of the subsystem
+   */
+  explicit Subsystem(const wpi::Twine& name);
+
+  Subsystem(Subsystem&&) = default;
+  Subsystem& operator=(Subsystem&&) = default;
+
+  /**
+   * Sets the default command. If this is not called or is called with null,
+   * then there will be no default command for the subsystem.
+   *
+   * <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the
+   * subsystem is a singleton.
+   *
+   * @param command the default command (or null if there should be none)
+   */
+  void SetDefaultCommand(Command* command);
+
+  /**
+   * Returns the default command (or null if there is none).
+   *
+   * @return the default command
+   */
+  Command* GetDefaultCommand();
+
+  /**
+   * Returns the default command name, or empty string is there is none.
+   *
+   * @return the default command name
+   */
+  wpi::StringRef GetDefaultCommandName();
+
+  /**
+   * Sets the current command.
+   *
+   * @param command the new current command
+   */
+  void SetCurrentCommand(Command* command);
+
+  /**
+   * Returns the command which currently claims this subsystem.
+   *
+   * @return the command which currently claims this subsystem
+   */
+  Command* GetCurrentCommand() const;
+
+  /**
+   * Returns the current command name, or empty string if no current command.
+   *
+   * @return the current command name
+   */
+  wpi::StringRef GetCurrentCommandName() const;
+
+  /**
+   * When the run method of the scheduler is called this method will be called.
+   */
+  virtual void Periodic();
+
+  /**
+   * Initialize the default command for this subsystem.
+   *
+   * This is meant to be the place to call SetDefaultCommand in a subsystem and
+   * will be called on all the subsystems by the CommandBase method before the
+   * program starts running by using the list of all registered Subsystems
+   * inside the Scheduler.
+   *
+   * This should be overridden by a Subsystem that has a default Command
+   */
+  virtual void InitDefaultCommand();
+
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  std::string GetName() const;
+
+  /**
+   * Sets the name of this Subsystem.
+   *
+   * @param name name
+   */
+  void SetName(const wpi::Twine& name);
+
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  std::string GetSubsystem() const;
+
+  /**
+   * Sets the subsystem name of this Subsystem.
+   *
+   * @param subsystem subsystem name
+   */
+  void SetSubsystem(const wpi::Twine& subsystem);
+
+  /**
+   * Associate a Sendable with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  void AddChild(const wpi::Twine& name, std::shared_ptr<Sendable> child);
+
+  /**
+   * Associate a Sendable with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  void AddChild(const wpi::Twine& name, Sendable* child);
+
+  /**
+   * Associate a Sendable with this Subsystem.
+   * Also update the child's name.
+   *
+   * @param name name to give child
+   * @param child sendable
+   */
+  void AddChild(const wpi::Twine& name, Sendable& child);
+
+  /**
+   * Associate a {@link Sendable} with this Subsystem.
+   *
+   * @param child sendable
+   */
+  void AddChild(std::shared_ptr<Sendable> child);
+
+  /**
+   * Associate a {@link Sendable} with this Subsystem.
+   *
+   * @param child sendable
+   */
+  void AddChild(Sendable* child);
+
+  /**
+   * Associate a {@link Sendable} with this Subsystem.
+   *
+   * @param child sendable
+   */
+  void AddChild(Sendable& child);
+
+ private:
+  /**
+   * Call this to alert Subsystem that the current command is actually the
+   * command.
+   *
+   * Sometimes, the Subsystem is told that it has no command while the Scheduler
+   * is going through the loop, only to be soon after given a new one. This will
+   * avoid that situation.
+   */
+  void ConfirmCommand();
+
+  Command* m_currentCommand = nullptr;
+  bool m_currentCommandChanged = true;
+  Command* m_defaultCommand = nullptr;
+  bool m_initializedDefaultCommand = false;
+
+ public:
+  void InitSendable(SendableBuilder& builder) override;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-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 "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * A TimedCommand will wait for a timeout before finishing.
+ *
+ * TimedCommand is used to execute a command for a given amount of time.
+ */
+class TimedCommand : public Command {
+ public:
+  /**
+   * Creates a new TimedCommand with the given name and timeout.
+   *
+   * @param name    the name of the command
+   * @param timeout the time (in seconds) before this command "times out"
+   */
+  TimedCommand(const wpi::Twine& name, double timeout);
+
+  /**
+   * Creates a new WaitCommand with the given timeout.
+   *
+   * @param timeout the time (in seconds) before this command "times out"
+   */
+  explicit TimedCommand(double timeout);
+
+  /**
+   * Creates a new TimedCommand with the given name and timeout.
+   *
+   * @param name      the name of the command
+   * @param timeout   the time (in seconds) before this command "times out"
+   * @param subsystem the subsystem that the command requires
+   */
+  TimedCommand(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+
+  /**
+   * Creates a new WaitCommand with the given timeout.
+   *
+   * @param timeout   the time (in seconds) before this command "times out"
+   * @param subsystem the subsystem that the command requires
+   */
+  TimedCommand(double timeout, Subsystem& subsystem);
+
+  virtual ~TimedCommand() = default;
+
+  TimedCommand(TimedCommand&&) = default;
+  TimedCommand& operator=(TimedCommand&&) = default;
+
+ protected:
+  /**
+   * Ends command when timed out.
+   */
+  bool IsFinished() override;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/TimedCommand.h"
+
+namespace frc {
+
+class WaitCommand : public TimedCommand {
+ public:
+  /**
+   * Creates a new WaitCommand with the given name and timeout.
+   *
+   * @param name    the name of the command
+   * @param timeout the time (in seconds) before this command "times out"
+   */
+  explicit WaitCommand(double timeout);
+
+  /**
+   * Creates a new WaitCommand with the given timeout.
+   *
+   * @param timeout the time (in seconds) before this command "times out"
+   */
+  WaitCommand(const wpi::Twine& name, double timeout);
+
+  virtual ~WaitCommand() = default;
+
+  WaitCommand(WaitCommand&&) = default;
+  WaitCommand& operator=(WaitCommand&&) = default;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class WaitForChildren : public Command {
+ public:
+  explicit WaitForChildren(double timeout);
+  WaitForChildren(const wpi::Twine& name, double timeout);
+  virtual ~WaitForChildren() = default;
+
+  WaitForChildren(WaitForChildren&&) = default;
+  WaitForChildren& operator=(WaitForChildren&&) = default;
+
+ protected:
+  virtual bool IsFinished();
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class WaitUntilCommand : public Command {
+ public:
+  /**
+   * A WaitCommand will wait until a certain match time before finishing.
+   *
+   * This will wait until the game clock reaches some value, then continue to
+   * the next command.
+   *
+   * @see CommandGroup
+   */
+  explicit WaitUntilCommand(double time);
+
+  WaitUntilCommand(const wpi::Twine& name, double time);
+
+  virtual ~WaitUntilCommand() = default;
+
+  WaitUntilCommand(WaitUntilCommand&&) = default;
+  WaitUntilCommand& operator=(WaitUntilCommand&&) = default;
+
+ protected:
+  /**
+   * Check if we've reached the actual finish time.
+   */
+  virtual bool IsFinished();
+
+ private:
+  double m_time;
+};
+
+}  // namespace frc