[commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392)

Moves all CommandBase functionality into Command and deprecates CommandBase for removal.
Moves all SubsystemBase functionality into Subsystem and deprecates SubsystemBase for removal.
Adds a function to CommandScheduler to remove all registered Subsystems.

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

@@ -4,6 +4,9 @@
 
 #include "frc2/command/Command.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
 #include "frc2/command/CommandHelper.h"
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -19,6 +22,10 @@
 
 using namespace frc2;
 
+Command::Command() {
+  wpi::SendableRegistry::Add(this, GetTypeName(*this));
+}
+
 Command::~Command() {
   CommandScheduler::GetInstance().Cancel(this);
 }
@@ -32,6 +39,42 @@ void Command::Initialize() {}
 void Command::Execute() {}
 void Command::End(bool interrupted) {}
 
+wpi::SmallSet<Subsystem*, 4> Command::GetRequirements() const {
+  return m_requirements;
+}
+
+void Command::AddRequirements(std::initializer_list<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(std::span<Subsystem* const> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(Subsystem* requirement) {
+  m_requirements.insert(requirement);
+}
+
+void Command::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
+}
+
+std::string Command::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+std::string Command::GetSubsystem() const {
+  return wpi::SendableRegistry::GetSubsystem(this);
+}
+
+void Command::SetSubsystem(std::string_view subsystem) {
+  wpi::SendableRegistry::SetSubsystem(this, subsystem);
+}
+
 CommandPtr Command::WithTimeout(units::second_t duration) && {
   return std::move(*this).ToPtr().WithTimeout(duration);
 }
@@ -125,12 +168,6 @@ bool Command::HasRequirement(Subsystem* requirement) const {
   return hasRequirement;
 }
 
-std::string Command::GetName() const {
-  return GetTypeName(*this);
-}
-
-void Command::SetName(std::string_view name) {}
-
 bool Command::IsComposed() const {
   return m_isComposed;
 }
@@ -139,6 +176,39 @@ void Command::SetComposed(bool isComposed) {
   m_isComposed = isComposed;
 }
 
+void Command::InitSendable(wpi::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();
+        }
+      });
+  builder.AddBooleanProperty(
+      ".isParented", [this] { return IsComposed(); }, nullptr);
+  builder.AddStringProperty(
+      "interruptBehavior",
+      [this] {
+        switch (GetInterruptionBehavior()) {
+          case Command::InterruptionBehavior::kCancelIncoming:
+            return "kCancelIncoming";
+          case Command::InterruptionBehavior::kCancelSelf:
+            return "kCancelSelf";
+          default:
+            return "Invalid";
+        }
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr);
+}
+
 namespace frc2 {
 bool RequirementsDisjoint(Command* first, Command* second) {
   bool disjoint = true;