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