Subsystem Disable Option

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

@@ -23,12 +23,30 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public abstract class Subsystem implements Sendable {
+  /** Whether the subsystem is enabled. */
+  private boolean m_enabled;
+
+  /** The default command. */
+  private Command m_defaultCommand;
+
   /** Constructor. */
   public Subsystem() {
+    this(true);
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param enabled Whether to enable the subsystem
+   */
+  public Subsystem(boolean enabled) {
     String name = this.getClass().getSimpleName();
     name = name.substring(name.lastIndexOf('.') + 1);
     SendableRegistry.addLW(this, name, name);
-    CommandScheduler.getInstance().registerSubsystem(this);
+    m_enabled = enabled;
+    if (enabled) {
+      CommandScheduler.getInstance().registerSubsystem(this);
+    }
   }
 
   /**
@@ -46,6 +64,20 @@ public void periodic() {}
    */
   public void simulationPeriodic() {}
 
+  /** Enables the subsystem. */
+  public void enable() {
+    register();
+    if (m_defaultCommand != null) {
+      setDefaultCommand(m_defaultCommand);
+    }
+  }
+
+  /** Disables the subsystem. */
+  public void disable() {
+    m_enabled = false;
+    CommandScheduler.getInstance().unregisterSubsystem(this);
+  }
+
   /**
    * Sets the default {@link Command} of the subsystem. The default command will be automatically
    * scheduled when no other commands are scheduled that require the subsystem. Default commands
@@ -56,15 +88,21 @@ public void simulationPeriodic() {}
    * @param defaultCommand the default command to associate with this subsystem
    */
   public void setDefaultCommand(Command defaultCommand) {
-    CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
+    if (m_enabled) {
+      CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
+    }
+    m_defaultCommand = defaultCommand;
   }
 
   /**
    * Removes the default command for the subsystem. This will not cancel the default command if it
    * is currently running.
    */
   public void removeDefaultCommand() {
-    CommandScheduler.getInstance().removeDefaultCommand(this);
+    if (m_enabled) {
+      CommandScheduler.getInstance().removeDefaultCommand(this);
+    }
+    m_defaultCommand = null;
   }
 
   /**
@@ -74,17 +112,17 @@ public void removeDefaultCommand() {
    * @return the default command associated with this subsystem
    */
   public Command getDefaultCommand() {
-    return CommandScheduler.getInstance().getDefaultCommand(this);
+    return m_defaultCommand;
   }
 
   /**
    * Returns the command currently running on this subsystem. Returns null if no command is
-   * currently scheduled that requires this subsystem.
+   * currently scheduled that requires this subsystem or if the subsystem is disabled.
    *
    * @return the scheduled command currently requiring this subsystem
    */
   public Command getCurrentCommand() {
-    return CommandScheduler.getInstance().requiring(this);
+    return m_enabled ? CommandScheduler.getInstance().requiring(this) : null;
   }
 
   /**
@@ -128,6 +166,7 @@ public void setSubsystem(String subsystem) {
    * Subsystem#periodic()} method to be called when the scheduler runs.
    */
   public void register() {
+    m_enabled = true;
     CommandScheduler.getInstance().registerSubsystem(this);
   }
 

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

@@ -9,34 +9,61 @@
 
 using namespace frc2;
 
-Subsystem::Subsystem() {
+Subsystem::Subsystem() : Subsystem::Subsystem(true) {}
+
+Subsystem::Subsystem(bool enabled) {
   wpi::SendableRegistry::AddLW(this, GetTypeName(*this));
-  CommandScheduler::GetInstance().RegisterSubsystem({this});
+  m_enabled = enabled;
+  m_defaultCommand = nullptr;
+  if (enabled) {
+    CommandScheduler::GetInstance().RegisterSubsystem({this});
+  }
 }
 
 Subsystem::~Subsystem() {
-  CommandScheduler::GetInstance().UnregisterSubsystem(this);
+  if (m_enabled) {
+    CommandScheduler::GetInstance().UnregisterSubsystem(this);
+  }
 }
 
 void Subsystem::Periodic() {}
 
 void Subsystem::SimulationPeriodic() {}
 
+void Subsystem::Enable() {
+  m_enabled = true;
+  Register();
+  if (m_defaultCommand != nullptr) {
+    SetDefaultCommand(std::move(*m_defaultCommand));
+  }
+}
+
+void Subsystem::Disable() {
+  m_enabled = false;
+  CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
 void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
-  CommandScheduler::GetInstance().SetDefaultCommand(this,
+  *m_defaultCommand = std::move(defaultCommand);
+  if (m_enabled) {
+    CommandScheduler::GetInstance().SetDefaultCommand(this,
                                                     std::move(defaultCommand));
+  }
 }
 
 void Subsystem::RemoveDefaultCommand() {
-  CommandScheduler::GetInstance().RemoveDefaultCommand(this);
+  if (m_enabled) {
+		CommandScheduler::GetInstance().RemoveDefaultCommand(this);
+	}
+  m_defaultCommand = nullptr;
 }
 
 Command* Subsystem::GetDefaultCommand() const {
-  return CommandScheduler::GetInstance().GetDefaultCommand(this);
+  return m_defaultCommand->get();
 }
 
 Command* Subsystem::GetCurrentCommand() const {
-  return CommandScheduler::GetInstance().Requiring(this);
+  return m_enabled ? CommandScheduler::GetInstance().Requiring(this) : nullptr;
 }
 
 std::string Subsystem::GetName() const {

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

@@ -37,6 +37,9 @@ class CommandPtr;
  * @see CommandScheduler
  */
 class Subsystem : public wpi::Sendable, public wpi::SendableHelper<Subsystem> {
+  bool m_enabled;
+  CommandPtr* m_defaultCommand;
+
  public:
   ~Subsystem() override;
 
@@ -57,6 +60,16 @@ class Subsystem : public wpi::Sendable, public wpi::SendableHelper<Subsystem> {
    */
   virtual void SimulationPeriodic();
 
+  /**
+  * Enables the subsystem
+  */
+  virtual void Enable();
+
+  /**
+  * Disables the subsystem
+  */
+  virtual void Disable();
+
   /**
    * Sets the default Command of the subsystem.  The default command will be
    * automatically scheduled when no other commands are scheduled that require
@@ -99,7 +112,8 @@ class Subsystem : public wpi::Sendable, public wpi::SendableHelper<Subsystem> {
 
   /**
    * Returns the command currently running on this subsystem.  Returns null if
-   * no command is currently scheduled that requires this subsystem.
+   * no command is currently scheduled that requires this subsystem or if the
+   * subsystem is disabled.
    *
    * @return the scheduled command currently requiring this subsystem
    */
@@ -190,5 +204,6 @@ class Subsystem : public wpi::Sendable, public wpi::SendableHelper<Subsystem> {
 
  protected:
   Subsystem();
+  Subsystem(bool enabled);
 };
 }  // namespace frc2