diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java index ea5d5f4e6c..5987149fe1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java @@ -315,6 +315,21 @@ public interface Command { return new ConditionalCommand(new InstantCommand(), this, condition); } + /** + * Decorates this command to run or stop when disabled. + * + * @param doesRunWhenDisabled true to run when disabled. + * @return the decorated command + */ + default WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) { + return new WrapperCommand(this) { + @Override + public boolean runsWhenDisabled() { + return doesRunWhenDisabled; + } + }; + } + /** * Schedules this command. * diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java new file mode 100644 index 0000000000..de1530c170 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java @@ -0,0 +1,107 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj2.command; + +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands; +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped; + +import java.util.Set; + +/** + * A class used internally to wrap commands while overriding a specific method; all other methods + * will call through to the wrapped command. + * + *

Wrapped commands may only be used through the wrapper, trying to directly schedule them or add + * them to a group will throw an exception. + */ +public abstract class WrapperCommand implements Command { + protected final Command m_command; + + /** + * Wrap a command. + * + * @param command the command being wrapped. Trying to directly schedule this command or add it to + * a group will throw an exception. + */ + protected WrapperCommand(Command command) { + requireUngrouped(command); + registerGroupedCommands(command); + m_command = command; + } + + /** The initial subroutine of a command. Called once when the command is initially scheduled. */ + @Override + public void initialize() { + m_command.initialize(); + } + + /** The main body of a command. Called repeatedly while the command is scheduled. */ + @Override + public void execute() { + m_command.execute(); + } + + /** + * The action to take when the command ends. Called when either the command finishes normally, or + * when it interrupted/canceled. + * + *

Do not schedule commands here that share requirements with this command. Use {@link + * #andThen(Command...)} instead. + * + * @param interrupted whether the command was interrupted/canceled + */ + @Override + public void end(boolean interrupted) { + m_command.end(interrupted); + } + + /** + * Whether the command has finished. Once a command finishes, the scheduler will call its end() + * method and un-schedule it. + * + * @return whether the command has finished. + */ + @Override + public boolean isFinished() { + return m_command.isFinished(); + } + + /** + * Specifies the set of subsystems used by this command. Two commands cannot use the same + * subsystem at the same time. If the command is scheduled as interruptible and another command is + * scheduled that shares a requirement, the command will be interrupted. Else, the command will + * not be scheduled. If no subsystems are required, return an empty set. + * + *

Note: it is recommended that user implementations contain the requirements as a field, and + * return that field here, rather than allocating a new set every time this is called. + * + * @return the set of subsystems that are required + */ + @Override + public Set getRequirements() { + return m_command.getRequirements(); + } + + /** + * Whether the given command should run when the robot is disabled. Override to return true if the + * command should run when disabled. + * + * @return whether the command should run when the robot is disabled + */ + @Override + public boolean runsWhenDisabled() { + return m_command.runsWhenDisabled(); + } + + /** + * Gets the name of this Command. + * + * @return Name + */ + @Override + public String getName() { + return m_command.getName(); + } +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index ff956ff7fa..cc62d4be39 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -4,6 +4,7 @@ #include "frc2/command/Command.h" +#include "frc2/command/CommandHelper.h" #include "frc2/command/CommandScheduler.h" #include "frc2/command/ConditionalCommand.h" #include "frc2/command/EndlessCommand.h" @@ -17,6 +18,7 @@ #include "frc2/command/SequentialCommandGroup.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" +#include "frc2/command/WrapperCommand.h" using namespace frc2; @@ -47,6 +49,24 @@ ParallelRaceGroup Command::Until(std::function condition) && { return ParallelRaceGroup(std::move(temp)); } +std::unique_ptr Command::IgnoringDisable(bool doesRunWhenDisabled) && { + class RunsWhenDisabledCommand + : public CommandHelper { + public: + RunsWhenDisabledCommand(std::unique_ptr&& command, + bool doesRunWhenDisabled) + : CommandHelper(std::move(command)), + m_runsWhenDisabled(doesRunWhenDisabled) {} + bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } + + private: + bool m_runsWhenDisabled; + }; + + return std::make_unique( + std::move(*this).TransferOwnership(), doesRunWhenDisabled); +} + ParallelRaceGroup Command::WithInterrupt(std::function condition) && { std::vector> temp; temp.emplace_back(std::make_unique(std::move(condition))); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp new file mode 100644 index 0000000000..0021d3208e --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc2/command/WrapperCommand.h" + +using namespace frc2; + +WrapperCommand::WrapperCommand(std::unique_ptr&& command) { + if (!CommandGroupBase::RequireUngrouped(*command)) { + return; + } + m_command = std::move(command); + m_command->SetGrouped(true); +} + +void WrapperCommand::Initialize() { + m_command->Initialize(); +} + +void WrapperCommand::Execute() { + m_command->Execute(); +} + +bool WrapperCommand::IsFinished() { + return m_command->IsFinished(); +} + +void WrapperCommand::End(bool interrupted) { + m_command->End(interrupted); +} + +bool WrapperCommand::RunsWhenDisabled() const { + return m_command->RunsWhenDisabled(); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 8a58e4d1b8..8eaef41b34 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -229,6 +229,14 @@ class Command { */ virtual ConditionalCommand Unless(std::function condition) &&; + /** + * Decorates this command to run or stop when disabled. + * + * @param doesRunWhenDisabled true to run when disabled. + * @return the decorated command + */ + virtual std::unique_ptr IgnoringDisable(bool doesRunWhenDisabled) &&; + /** * Schedules this command. * diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h new file mode 100644 index 0000000000..9516728ef3 --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4521) +#endif + +#include +#include + +#include "frc2/command/CommandBase.h" +#include "frc2/command/CommandGroupBase.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A class used internally to wrap commands while overriding a specific method; + * all other methods will call through to the wrapped command. + * + *

Wrapped commands may only be used through the wrapper, trying to directly + * schedule them or add them to a group will throw an exception. + */ +class WrapperCommand : public CommandHelper { + public: + /** + * Wrap a command. + * + * @param command the command being wrapped. Trying to directly schedule this + * command or add it to a group will throw an exception. + */ + explicit WrapperCommand(std::unique_ptr&& command); + + /** + * Wrap a command. + * + * @param command the command being wrapped. Trying to directly schedule this + * command or add it to a group will throw an exception. + */ + template >>> + explicit WrapperCommand(T&& command) + : WrapperCommand(std::make_unique>( + std::forward(command))) {} + + WrapperCommand(WrapperCommand&& other) = default; + + // No copy constructors for command groups + WrapperCommand(const WrapperCommand& other) = delete; + + // Prevent template expansion from emulating copy ctor + WrapperCommand(WrapperCommand&) = delete; + + void Initialize() override; + + void Execute() override; + + bool IsFinished() override; + + void End(bool interrupted) override; + + bool RunsWhenDisabled() const override; + + protected: + std::unique_ptr m_command; +}; +} // namespace frc2 + +#ifdef _WIN32 +#pragma warning(pop) +#endif diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java index 2ab1981a74..3cba1e59a6 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java @@ -55,6 +55,20 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void ignoringDisableTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + var command = new RunCommand(() -> {}).ignoringDisable(true); + + setDSEnabled(false); + + scheduler.schedule(command); + + scheduler.run(); + assertTrue(scheduler.isScheduled(command)); + } + } + @Test void beforeStartingTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index 801fb3dfe9..22f5fe263e 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -54,6 +54,19 @@ TEST_F(CommandDecoratorTest, Until) { EXPECT_FALSE(scheduler.IsScheduled(&command)); } +TEST_F(CommandDecoratorTest, IgnoringDisable) { + CommandScheduler scheduler = GetScheduler(); + + auto command = RunCommand([] {}, {}).IgnoringDisable(true); + + SetDSEnabled(false); + + scheduler.Schedule(command.get()); + + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(command.get())); +} + TEST_F(CommandDecoratorTest, BeforeStarting) { CommandScheduler scheduler = GetScheduler();