[commands] Subsystem: Add default command removal method (#5064)

Author: democat3457

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java

@@ -50,6 +50,14 @@ default void setDefaultCommand(Command defaultCommand) {
     CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
   }
 
+  /**
+   * Removes the default command for the subsystem. This will not cancel the default command if it
+   * is currently running.
+   */
+  default void removeDefaultCommand() {
+    CommandScheduler.getInstance().removeDefaultCommand(this);
+  }
+
   /**
    * Gets the default command for this subsystem. Returns null if no default command is currently
    * associated with the subsystem.

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

@@ -21,6 +21,10 @@ void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
                                                     std::move(defaultCommand));
 }
 
+void Subsystem::RemoveDefaultCommand() {
+  CommandScheduler::GetInstance().RemoveDefaultCommand(this);
+}
+
 Command* Subsystem::GetDefaultCommand() const {
   return CommandScheduler::GetInstance().GetDefaultCommand(this);
 }

wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h

@@ -83,6 +83,12 @@ class Subsystem {
    */
   void SetDefaultCommand(CommandPtr&& defaultCommand);
 
+  /**
+   * Removes the default command for the subsystem.  This will not cancel the
+   * default command if it is currently running.
+   */
+  void RemoveDefaultCommand();
+
   /**
    * Gets the default command for this subsystem.  Returns null if no default
    * command is currently associated with the subsystem.