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.

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

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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

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

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <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();
+                               }
+                             });
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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(); }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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);
+}

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

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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());
+}

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

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "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; }

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

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

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

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

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

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