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 b37a17054e..c5466a542e 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 @@ -72,7 +72,7 @@ public interface Command { * @return the command with the timeout added */ default ParallelRaceGroup withTimeout(double seconds) { - return new ParallelRaceGroup(this, new WaitCommand(seconds)); + return raceWith(new WaitCommand(seconds)); } /** @@ -91,7 +91,7 @@ public interface Command { * @return the command with the interrupt condition added */ default ParallelRaceGroup withInterrupt(BooleanSupplier condition) { - return new ParallelRaceGroup(this, new WaitUntilCommand(condition)); + return raceWith(new WaitUntilCommand(condition)); } /** @@ -108,7 +108,23 @@ public interface Command { * @return the decorated command */ default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) { - return new SequentialCommandGroup(new InstantCommand(toRun, requirements), this); + return beforeStarting(new InstantCommand(toRun, requirements)); + } + + /** + * Decorates this command with another command to run before this command starts. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with {@link + * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further + * decorated without issue. + * + * @param before the command to run before this one + * @return the decorated command + */ + default SequentialCommandGroup beforeStarting(Command before) { + return new SequentialCommandGroup(before, this); } /** @@ -125,7 +141,7 @@ public interface Command { * @return the decorated command */ default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) { - return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements)); + return andThen(new InstantCommand(toRun, requirements)); } /** @@ -266,10 +282,7 @@ public interface Command { } /** - * Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires" - * to avoid confusion with {@link - * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)} - this - * may be able to be changed in a few years. + * Whether the command requires a given subsystem. * * @param requirement the subsystem to inquire about * @return whether the subsystem is required diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java index 5c6fe1ee66..7195606e19 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java @@ -86,11 +86,17 @@ public class ParallelCommandGroup extends CommandGroupBase { @Override public boolean isFinished() { - return !m_commands.values().contains(true); + return !m_commands.containsValue(true); } @Override public boolean runsWhenDisabled() { return m_runWhenDisabled; } + + @Override + public ParallelCommandGroup alongWith(Command... parallel) { + addCommands(parallel); + return this; + } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java index 06539f0f52..d444991742 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java @@ -117,4 +117,10 @@ public class ParallelDeadlineGroup extends CommandGroupBase { public boolean runsWhenDisabled() { return m_runWhenDisabled; } + + @Override + public ParallelDeadlineGroup deadlineWith(Command... parallel) { + addCommands(parallel); + return this; + } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java index 434ee38fcd..2d798852f5 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java @@ -86,4 +86,10 @@ public class ParallelRaceGroup extends CommandGroupBase { public boolean runsWhenDisabled() { return m_runWhenDisabled; } + + @Override + public ParallelRaceGroup raceWith(Command... parallel) { + addCommands(parallel); + return this; + } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java index 9291f64a5f..af219dc523 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java @@ -50,4 +50,9 @@ public class PerpetualCommand extends CommandBase { public boolean runsWhenDisabled() { return m_command.runsWhenDisabled(); } + + @Override + public PerpetualCommand perpetually() { + return this; + } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java index 492e3169e1..38dff8d228 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java @@ -92,4 +92,28 @@ public class SequentialCommandGroup extends CommandGroupBase { public boolean runsWhenDisabled() { return m_runWhenDisabled; } + + @Override + public SequentialCommandGroup beforeStarting(Command before) { + // store all the commands + var commands = new ArrayList(); + commands.add(before); + commands.addAll(m_commands); + + // reset current state + commands.forEach(CommandGroupBase::clearGroupedCommand); + m_commands.clear(); + m_requirements.clear(); + m_runWhenDisabled = true; + + // add them back + addCommands(commands.toArray(Command[]::new)); + return this; + } + + @Override + public SequentialCommandGroup andThen(Command... next) { + addCommands(next); + return this; + } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp index 0c199f2d05..bd8bb93ed3 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp @@ -26,3 +26,7 @@ void PerpetualCommand::Execute() { void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); } + +PerpetualCommand PerpetualCommand::Perpetually() && { + return std::move(*this); +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp index 1de96a3300..c7f409bfa6 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp @@ -4,6 +4,8 @@ #include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/InstantCommand.h" + using namespace frc2; SequentialCommandGroup::SequentialCommandGroup( @@ -72,3 +74,33 @@ void SequentialCommandGroup::AddCommands( m_commands.emplace_back(std::move(command)); } } + +SequentialCommandGroup SequentialCommandGroup::BeforeStarting( + std::function toRun, wpi::ArrayRef requirements) && { + // store all the commands + std::vector> tmp; + tmp.emplace_back( + std::make_unique(std::move(toRun), requirements)); + for (auto&& command : m_commands) { + command->SetGrouped(false); + tmp.emplace_back(std::move(command)); + } + + // reset current state + m_commands.clear(); + m_requirements.clear(); + m_runWhenDisabled = true; + + // add the commands back + AddCommands(std::move(tmp)); + return std::move(*this); +} + +SequentialCommandGroup SequentialCommandGroup::AndThen( + std::function toRun, wpi::ArrayRef requirements) && { + std::vector> tmp; + tmp.emplace_back( + std::make_unique(std::move(toRun), requirements)); + AddCommands(std::move(tmp)); + return std::move(*this); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index e9b5e4afde..9f507b5fb3 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -107,7 +107,7 @@ class Command { * @param duration the timeout duration * @return the command with the timeout added */ - ParallelRaceGroup WithTimeout(units::second_t duration) &&; + virtual ParallelRaceGroup WithTimeout(units::second_t duration) &&; /** * Decorates this command with an interrupt condition. If the specified @@ -118,7 +118,7 @@ class Command { * @param condition the interrupt condition * @return the command with the interrupt condition added */ - ParallelRaceGroup WithInterrupt(std::function condition) &&; + virtual ParallelRaceGroup WithInterrupt(std::function condition) &&; /** * Decorates this command with a runnable to run before this command starts. @@ -127,7 +127,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup BeforeStarting( + virtual SequentialCommandGroup BeforeStarting( std::function toRun, std::initializer_list requirements) &&; @@ -138,7 +138,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup BeforeStarting( + virtual SequentialCommandGroup BeforeStarting( std::function toRun, wpi::span requirements = {}) &&; @@ -149,7 +149,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup AndThen( + virtual SequentialCommandGroup AndThen( std::function toRun, std::initializer_list requirements) &&; @@ -160,7 +160,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup AndThen( + virtual SequentialCommandGroup AndThen( std::function toRun, wpi::span requirements = {}) &&; @@ -170,7 +170,7 @@ class Command { * * @return the decorated command */ - PerpetualCommand Perpetually() &&; + virtual PerpetualCommand Perpetually() &&; /** * Decorates this command to run "by proxy" by wrapping it in a @@ -180,7 +180,7 @@ class Command { * * @return the decorated command */ - ProxyScheduleCommand AsProxy(); + virtual ProxyScheduleCommand AsProxy(); /** * Schedules this command. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h index 63108ecc02..a288ae23b4 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h @@ -65,6 +65,8 @@ class PerpetualCommand : public CommandHelper { void End(bool interrupted) override; + PerpetualCommand Perpetually() && override; + private: std::unique_ptr m_command; }; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h index 4b09544b12..144f98d86a 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h @@ -84,6 +84,15 @@ class SequentialCommandGroup bool RunsWhenDisabled() const override; + SequentialCommandGroup BeforeStarting( + std::function toRun, + wpi::ArrayRef requirements = {}) && + override; + + SequentialCommandGroup AndThen(std::function toRun, + wpi::ArrayRef requirements = {}) && + override; + private: void AddCommands(std::vector>&& commands) final; diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java index 8ab4a6d7bb..7cf1a28543 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import static org.mockito.ArgumentMatchers.anyDouble; +import static org.mockito.ArgumentMatchers.notNull; import static org.mockito.Mockito.never; import static org.mockito.Mockito.verify; import static org.mockito.Mockito.when; @@ -58,6 +59,7 @@ class WaitCommandTest extends CommandTestBase { MockCommandHolder command1Holder = new MockCommandHolder(true); Command command1 = command1Holder.getMock(); when(command1.withTimeout(anyDouble())).thenCallRealMethod(); + when(command1.raceWith(notNull())).thenCallRealMethod(); Command timeout = command1.withTimeout(2);