diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index d7c3754fb8..2a73713e3a 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/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()) { diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp index 51cbddf8cc..267a818979 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp +++ b/wpilibNewCommands/src/main/native/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); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index 095b8b5338..fe1dae7cf7 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -191,6 +191,19 @@ class CommandScheduler final : public nt::NTSendable, std::forward(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. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index 7ee99505b3..67fc08db32 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/wpilibNewCommands/src/main/native/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(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.