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.

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -16,7 +16,6 @@
 
 #include "frc/DriverStation.h"
 #include "frc/Timer.h"
-#include "frc/commands/Scheduler.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -131,7 +130,6 @@ void IterativeRobotBase::LoopFunc() {
       TeleopInit();
       m_watchdog.AddEpoch("TeleopInit()");
       m_lastMode = Mode::kTeleop;
-      Scheduler::GetInstance()->SetEnabled(true);
     }
 
     HAL_ObserveUserProgramTeleop();

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

@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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); }

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

@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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); }

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

@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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; }

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

@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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); }

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

@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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);
-}

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

@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 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; }

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

@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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; });
-}

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

@@ -1,264 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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);
-}

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

@@ -1,243 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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++;
-  }
-}

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

@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 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();
-}

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

@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 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; }

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

@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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");
-}

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

@@ -1,97 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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'; }

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

@@ -1,245 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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;
-  }
-}

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

@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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(); }

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

@@ -1,134 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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);
-}

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

@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 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(); }

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

@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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) {}

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

@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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;
-}

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

@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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; }

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

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

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

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

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

@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandGroupBase.h"
-
-#include <set>
-
-#include "frc/WPIErrors.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
-  return map.find(keyToCheck) != map.end();
-}
-bool CommandGroupBase::RequireUngrouped(Command& command) {
-  if (command.IsGrouped()) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-    return false;
-  } else {
-    return true;
-  }
-}
-
-bool CommandGroupBase::RequireUngrouped(
-    wpi::ArrayRef<std::unique_ptr<Command>> commands) {
-  bool allUngrouped = true;
-  for (auto&& command : commands) {
-    allUngrouped &= !command.get()->IsGrouped();
-  }
-  if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-  }
-  return allUngrouped;
-}
-
-bool CommandGroupBase::RequireUngrouped(
-    std::initializer_list<Command*> commands) {
-  bool allUngrouped = true;
-  for (auto&& command : commands) {
-    allUngrouped &= !command->IsGrouped();
-  }
-  if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-  }
-  return allUngrouped;
-}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/PrintCommand.h"
-
-using namespace frc2;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
-    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }

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

@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDCommand.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<State()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : m_controller{controller},
-      m_measurement{std::move(measurementSource)},
-      m_goal{std::move(goalSource)},
-      m_useOutput{std::move(useOutput)} {
-  AddRequirements(requirements);
-}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<units::meter_t()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource,
-                         [&goalSource]() {
-                           return State{goalSource(), 0_mps};
-                         },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, State goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, units::meter_t goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
-
-void ProfiledPIDCommand::Execute() {
-  m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
-              m_controller.GetSetpoint());
-}
-
-void ProfiledPIDCommand::End(bool interrupted) {
-  m_useOutput(0, State{0_m, 0_mps});
-}
-
-frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
-  return m_controller;
-}

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

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

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

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

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

@@ -1,117 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/RamseteCommand.h"
-
-using namespace frc2;
-using namespace units;
-
-template <typename T>
-int sgn(T val) {
-  return (T(0) < val) - (val < T(0));
-}
-
-RamseteCommand::RamseteCommand(
-    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-    frc::RamseteController controller, volt_t ks,
-    units::unit_t<voltsecondspermeter> kv,
-    units::unit_t<voltsecondssquaredpermeter> ka,
-    frc::DifferentialDriveKinematics kinematics,
-    std::function<units::meters_per_second_t()> leftSpeed,
-    std::function<units::meters_per_second_t()> rightSpeed,
-    frc2::PIDController leftController, frc2::PIDController rightController,
-    std::function<void(volt_t, volt_t)> output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
-      m_controller(controller),
-      m_ks(ks),
-      m_kv(kv),
-      m_ka(ka),
-      m_kinematics(kinematics),
-      m_leftSpeed(leftSpeed),
-      m_rightSpeed(rightSpeed),
-      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
-      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
-      m_outputVolts(output),
-      m_usePID(true) {
-  AddRequirements(requirements);
-}
-
-RamseteCommand::RamseteCommand(
-    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-    frc::RamseteController controller,
-    frc::DifferentialDriveKinematics kinematics,
-    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
-        output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
-      m_controller(controller),
-      m_ks(0),
-      m_kv(0),
-      m_ka(0),
-      m_kinematics(kinematics),
-      m_outputVel(output),
-      m_usePID(false) {
-  AddRequirements(requirements);
-}
-
-void RamseteCommand::Initialize() {
-  m_prevTime = 0_s;
-  auto initialState = m_trajectory.Sample(0_s);
-  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
-      frc::ChassisSpeeds{initialState.velocity, 0_mps,
-                         initialState.velocity * initialState.curvature});
-  m_timer.Reset();
-  m_timer.Start();
-  if (m_usePID) {
-    m_leftController->Reset();
-    m_rightController->Reset();
-  }
-}
-
-void RamseteCommand::Execute() {
-  auto curTime = m_timer.Get();
-  auto dt = curTime - m_prevTime;
-
-  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
-      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
-
-  if (m_usePID) {
-    auto leftFeedforward =
-        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
-        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
-
-    auto rightFeedforward =
-        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
-        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
-
-    auto leftOutput =
-        volt_t(m_leftController->Calculate(
-            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
-        leftFeedforward;
-
-    auto rightOutput = volt_t(m_rightController->Calculate(
-                           m_rightSpeed().to<double>(),
-                           targetWheelSpeeds.right.to<double>())) +
-                       rightFeedforward;
-
-    m_outputVolts(leftOutput, rightOutput);
-  } else {
-    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
-  }
-
-  m_prevTime = curTime;
-  m_prevSpeeds = targetWheelSpeeds;
-}
-
-void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool RamseteCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
-}

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/TrapezoidProfileCommand.h"
-
-#include <units/units.h>
-
-using namespace frc2;
-
-TrapezoidProfileCommand::TrapezoidProfileCommand(
-    frc::TrapezoidProfile profile,
-    std::function<void(frc::TrapezoidProfile::State)> output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_profile(profile), m_output(output) {
-  AddRequirements(requirements);
-}
-
-void TrapezoidProfileCommand::Initialize() {
-  m_timer.Reset();
-  m_timer.Start();
-}
-
-void TrapezoidProfileCommand::Execute() {
-  m_output(m_profile.Calculate(m_timer.Get()));
-}
-
-void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool TrapezoidProfileCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_profile.TotalTime());
-}

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

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

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

@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-
-WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
-    : m_condition{std::move(condition)} {}
-
-WaitUntilCommand::WaitUntilCommand(double time)
-    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
-
-bool WaitUntilCommand::IsFinished() { return m_condition(); }
-
-bool WaitUntilCommand::RunsWhenDisabled() const { return true; }

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

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

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

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

wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp

@@ -12,7 +12,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/mutex.h>
 
-#include "frc/commands/Scheduler.h"
+#include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilderImpl.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -99,19 +99,15 @@ bool LiveWindow::IsEnabled() const {
 void LiveWindow::SetEnabled(bool enabled) {
   std::scoped_lock lock(m_impl->mutex);
   if (m_impl->liveWindowEnabled == enabled) return;
-  Scheduler* scheduler = Scheduler::GetInstance();
   m_impl->startLiveWindow = enabled;
   m_impl->liveWindowEnabled = enabled;
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
-    scheduler->SetEnabled(false);
-    scheduler->RemoveAll();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
-    scheduler->SetEnabled(true);
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }

wpilibc/src/main/native/include/frc/WPILib.h

@@ -90,7 +90,6 @@
 #include "frc/commands/PIDCommand.h"
 #include "frc/commands/PIDSubsystem.h"
 #include "frc/commands/PrintCommand.h"
-#include "frc/commands/Scheduler.h"
 #include "frc/commands/StartCommand.h"
 #include "frc/commands/Subsystem.h"
 #include "frc/commands/WaitCommand.h"

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

@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 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

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

@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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

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

@@ -1,492 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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

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

@@ -1,180 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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

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

@@ -1,93 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 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

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

@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,236 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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

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

@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,199 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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

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

@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 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

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

@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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