Merge branch 'upstream' into main

Author: virtuald

commands2/src/cpp/frc2/command/Command.cpp

@@ -0,0 +1,133 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/Command.h"
+
+#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::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).BeforeStarting(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::BeforeStarting(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::AndThen(
+    std::function<void()> toRun,
+    std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).AndThen(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::AndThen(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(*this).TransferOwnership());
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  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

commands2/src/cpp/frc2/command/CommandBase.cpp

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+
+using namespace frc2;
+
+CommandBase::CommandBase() {
+  frc::SendableRegistry::GetInstance().Add(this, GetTypeName(*this));
+}
+
+void CommandBase::AddRequirements(
+    std::initializer_list<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void CommandBase::AddRequirements(wpi::ArrayRef<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();
+        }
+      });
+}

commands2/src/cpp/frc2/command/CommandGroupBase.cpp

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/CommandGroupBase.h"
+
+#include <frc/WPIErrors.h>
+
+using namespace frc2;
+
+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;
+}