Add new C++ Command framework (#1785)

This is the C++ version of #1682.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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