[commands] C++: Add CommandPtr overload for SetDefaultCommand (#4488)

Author: Starlight220

cpp/frc2/command/CommandScheduler.cpp

@@ -296,6 +296,20 @@ void CommandScheduler::UnregisterSubsystem(
   }
 }
 
+void CommandScheduler::SetDefaultCommand(Subsystem* subsystem,
+                                         CommandPtr&& defaultCommand) {
+  if (!defaultCommand.get()->HasRequirement(subsystem)) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Default commands must require their subsystem!");
+  }
+  if (defaultCommand.get()->IsFinished()) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Default commands should not end!");
+  }
+
+  SetDefaultCommandImpl(subsystem, std::move(defaultCommand).Unwrap());
+}
+
 Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
   auto&& find = m_impl->subsystems.find(subsystem);
   if (find != m_impl->subsystems.end()) {

cpp/frc2/command/Subsystem.cpp

@@ -4,6 +4,8 @@
 
 #include "frc2/command/Subsystem.h"
 
+#include "frc2/command/CommandPtr.h"
+
 using namespace frc2;
 Subsystem::~Subsystem() {
   CommandScheduler::GetInstance().UnregisterSubsystem(this);
@@ -13,6 +15,11 @@ void Subsystem::Periodic() {}
 
 void Subsystem::SimulationPeriodic() {}
 
+void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
+  CommandScheduler::GetInstance().SetDefaultCommand(this,
+                                                    std::move(defaultCommand));
+}
+
 Command* Subsystem::GetDefaultCommand() const {
   return CommandScheduler::GetInstance().GetDefaultCommand(this);
 }

include/frc2/command/CommandScheduler.h

@@ -191,6 +191,19 @@ class CommandScheduler final : public nt::NTSendable,
                               std::forward<T>(defaultCommand)));
   }
 
+  /**
+   * Sets the default command for a subsystem.  Registers that subsystem if it
+   * is not already registered.  Default commands will run whenever there is no
+   * other command currently scheduled that requires the subsystem.  Default
+   * commands should be written to never end (i.e. their IsFinished() method
+   * should return false), as they would simply be re-scheduled if they do.
+   * Default commands must also require their subsystem.
+   *
+   * @param subsystem      the subsystem whose default command will be set
+   * @param defaultCommand the default command to associate with the subsystem
+   */
+  void SetDefaultCommand(Subsystem* subsystem, CommandPtr&& defaultCommand);
+
   /**
    * Gets the default command associated with this subsystem.  Null if this
    * subsystem has no default command associated with it.

include/frc2/command/Subsystem.h

@@ -11,6 +11,7 @@
 
 namespace frc2 {
 class Command;
+class CommandPtr;
 /**
  * A robot subsystem.  Subsystems are the basic unit of robot organization in
  * the Command-based framework; they encapsulate low-level hardware objects
@@ -71,6 +72,17 @@ class Subsystem {
         this, std::forward<T>(defaultCommand));
   }
 
+  /**
+   * Sets the default Command of the subsystem.  The default command will be
+   * automatically scheduled when no other commands are scheduled that require
+   * the subsystem. Default commands should generally not end on their own, i.e.
+   * their IsFinished() method should always return false.  Will automatically
+   * register this subsystem with the CommandScheduler.
+   *
+   * @param defaultCommand the default command to associate with this subsystem
+   */
+  void SetDefaultCommand(CommandPtr&& defaultCommand);
+
   /**
    * Gets the default command for this subsystem.  Returns null if no default
    * command is currently associated with the subsystem.