From bcd6c60c2e380deaa17fc8be4e081916b77f410e Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Thu, 17 Aug 2023 15:44:28 -0700 Subject: [PATCH 1/2] Implemented Subsystem Disable Option for Java --- .../wpi/first/wpilibj2/command/Subsystem.java | 51 ++++++++++++++++--- 1 file changed, 45 insertions(+), 6 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java index a14192521de..1688ad99091 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java @@ -23,12 +23,30 @@ *

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,7 +88,10 @@ 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; } /** @@ -64,7 +99,10 @@ public void setDefaultCommand(Command defaultCommand) { * 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); } From 9498ad97eb49abf002ce6bcbd84277cc06d13193 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Thu, 17 Aug 2023 17:07:30 -0700 Subject: [PATCH 2/2] Implemented Subsystem Disable Option for C++ --- .../native/cpp/frc2/command/Subsystem.cpp | 41 +++++++++++++++---- .../native/include/frc2/command/Subsystem.h | 17 +++++++- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp index 9fc6e8adac9..b3e542faee0 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp +++ b/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 { diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index 791db7096c1..a033e9ae241 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/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 { + bool m_enabled; + CommandPtr* m_defaultCommand; + public: ~Subsystem() override; @@ -57,6 +60,16 @@ class Subsystem : public wpi::Sendable, public wpi::SendableHelper { */ 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 { /** * 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 { protected: Subsystem(); + Subsystem(bool enabled); }; } // namespace frc2