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 82c6bc4995..0420301ead 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 @@ -257,6 +257,22 @@ public interface Command { return new PerpetualCommand(this); } + /** + * Decorates this command to run repeatedly, restarting it when it ends, until this command is + * interrupted. The decorated command can still be canceled. + * + *

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. + * + * @return the decorated command + */ + default RepeatCommand repeat() { + return new RepeatCommand(this); + } + /** * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}. This * is useful for "forking off" from command groups when the user does not wish to extend the diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java new file mode 100644 index 0000000000..f228fa7fa1 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java @@ -0,0 +1,70 @@ +// 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; + +/** + * A command that runs another command repeatedly, restarting it when it ends, until this command is + * interrupted. While this class does not extend {@link CommandGroupBase}, it is still considered a + * CommandGroup, as it allows one to compose another command within it; the command instances that + * are passed to it cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + * + *

This class is provided by the NewCommands VendorDep + */ +public class RepeatCommand extends CommandBase { + protected final Command m_command; + + /** + * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it + * ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + public RepeatCommand(Command command) { + requireUngrouped(command); + registerGroupedCommands(command); + m_command = command; + m_requirements.addAll(command.getRequirements()); + } + + @Override + public void initialize() { + m_command.initialize(); + } + + @Override + public void execute() { + m_command.execute(); + if (m_command.isFinished()) { + // restart command + m_command.end(false); + m_command.initialize(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + m_command.end(interrupted); + } + + @Override + public boolean runsWhenDisabled() { + return m_command.runsWhenDisabled(); + } + + @Override + public RepeatCommand repeat() { + return this; + } +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index ea6276b50b..93510513d0 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -11,6 +11,7 @@ #include "frc2/command/ParallelRaceGroup.h" #include "frc2/command/PerpetualCommand.h" #include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/RepeatCommand.h" #include "frc2/command/SequentialCommandGroup.h" #include "frc2/command/WaitCommand.h" #include "frc2/command/WaitUntilCommand.h" @@ -87,6 +88,10 @@ PerpetualCommand Command::Perpetually() && { return PerpetualCommand(std::move(*this).TransferOwnership()); } +RepeatCommand Command::Repeat() && { + return RepeatCommand(std::move(*this).TransferOwnership()); +} + ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp new file mode 100644 index 0000000000..023ccb157f --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp @@ -0,0 +1,45 @@ +// 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/RepeatCommand.h" + +using namespace frc2; + +RepeatCommand::RepeatCommand(std::unique_ptr&& command) { + if (!CommandGroupBase::RequireUngrouped(*command)) { + return; + } + m_command = std::move(command); + m_command->SetGrouped(true); + AddRequirements(m_command->GetRequirements()); +} + +void RepeatCommand::Initialize() { + m_command->Initialize(); +} + +void RepeatCommand::Execute() { + m_command->Execute(); + if (m_command->IsFinished()) { + // restart command + m_command->End(false); + m_command->Initialize(); + } +} + +bool RepeatCommand::IsFinished() { + return false; +} + +void RepeatCommand::End(bool interrupted) { + m_command->End(interrupted); +} + +bool RepeatCommand::RunsWhenDisabled() const { + return m_command->RunsWhenDisabled(); +} + +RepeatCommand RepeatCommand::Repeat() && { + 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 f8da72815b..bfbe516181 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -29,6 +29,7 @@ class ParallelDeadlineGroup; class SequentialCommandGroup; class PerpetualCommand; class ProxyScheduleCommand; +class RepeatCommand; /** * A state machine representing a complete action to be performed by the robot. @@ -185,6 +186,14 @@ class Command { */ virtual PerpetualCommand Perpetually() &&; + /** + * Decorates this command to run repeatedly, restarting it when it ends, until + * this command is interrupted. The decorated command can still be canceled. + * + * @return the decorated command + */ + virtual RepeatCommand Repeat() &&; + /** * Decorates this command to run "by proxy" by wrapping it in a * ProxyScheduleCommand. This is useful for "forking off" from command groups diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h new file mode 100644 index 0000000000..1d9a1f55cc --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RepeatCommand.h @@ -0,0 +1,81 @@ +// 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 command that runs another command repeatedly, restarting it when it ends, + * until this command is interrupted. While this class does not extend {@link + * CommandGroupBase}, it is still considered a CommandGroup, as it allows one to + * compose another command within it; the command instances that are passed to + * it cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + * + *

This class is provided by the NewCommands VendorDep + */ +class RepeatCommand : public CommandHelper { + public: + /** + * Creates a new RepeatCommand. Will run another command repeatedly, + * restarting it whenever it ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + explicit RepeatCommand(std::unique_ptr&& command); + + /** + * Creates a new RepeatCommand. Will run another command repeatedly, + * restarting it whenever it ends, until this command is interrupted. + * + * @param command the command to run repeatedly + */ + template >>> + explicit RepeatCommand(T&& command) + : RepeatCommand(std::make_unique>( + std::forward(command))) {} + + RepeatCommand(RepeatCommand&& other) = default; + + // No copy constructors for command groups + RepeatCommand(const RepeatCommand& other) = delete; + + // Prevent template expansion from emulating copy ctor + RepeatCommand(RepeatCommand&) = delete; + + void Initialize() override; + + void Execute() override; + + bool IsFinished() override; + + void End(bool interrupted) override; + + bool RunsWhenDisabled() const override; + + RepeatCommand Repeat() && override; + + private: + 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/RepeatCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java new file mode 100644 index 0000000000..7c4d416430 --- /dev/null +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java @@ -0,0 +1,71 @@ +// 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 org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class RepeatCommandTest { + @Test + void callsMethodsCorrectly() { + HAL.initialize(500, 0); + // enable so that we don't need to mess with `runsWhenDisabled` for each command + DriverStationSim.setEnabled(true); + + var initCounter = new AtomicInteger(0); + var exeCounter = new AtomicInteger(0); + var isFinishedCounter = new AtomicInteger(0); + var endCounter = new AtomicInteger(0); + var isFinishedHook = new AtomicBoolean(false); + + final var command = + new RepeatCommand( + new FunctionalCommand( + initCounter::incrementAndGet, + exeCounter::incrementAndGet, + interrupted -> endCounter.incrementAndGet(), + () -> { + isFinishedCounter.incrementAndGet(); + return isFinishedHook.get(); + })); + + assertEquals(0, initCounter.get()); + assertEquals(0, exeCounter.get()); + assertEquals(0, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + CommandScheduler.getInstance().schedule(command); + assertEquals(1, initCounter.get()); + assertEquals(0, exeCounter.get()); + assertEquals(0, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + isFinishedHook.set(false); + CommandScheduler.getInstance().run(); + assertEquals(1, initCounter.get()); + assertEquals(1, exeCounter.get()); + assertEquals(1, isFinishedCounter.get()); + assertEquals(0, endCounter.get()); + + isFinishedHook.set(true); + CommandScheduler.getInstance().run(); + assertEquals(2, initCounter.get()); + assertEquals(2, exeCounter.get()); + assertEquals(2, isFinishedCounter.get()); + assertEquals(1, endCounter.get()); + + isFinishedHook.set(false); + CommandScheduler.getInstance().run(); + assertEquals(2, initCounter.get()); + assertEquals(3, exeCounter.get()); + assertEquals(3, isFinishedCounter.get()); + assertEquals(1, endCounter.get()); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp new file mode 100644 index 0000000000..8d1f4845bd --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp @@ -0,0 +1,62 @@ +// 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 "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" +#include "frc2/command/RepeatCommand.h" + +using namespace frc2; +class RepeatCommandTest : public CommandTestBase {}; + +TEST_F(RepeatCommandTest, CallsMethodsCorrectly) { + CommandScheduler scheduler = GetScheduler(); + + int initCounter = 0; + int exeCounter = 0; + int isFinishedCounter = 0; + int endCounter = 0; + bool isFinishedHook = false; + + auto command = + FunctionalCommand([&initCounter] { initCounter++; }, + [&exeCounter] { exeCounter++; }, + [&endCounter](bool interrupted) { endCounter++; }, + [&isFinishedCounter, &isFinishedHook] { + isFinishedCounter++; + return isFinishedHook; + }) + .Repeat(); + + EXPECT_EQ(0, initCounter); + EXPECT_EQ(0, exeCounter); + EXPECT_EQ(0, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + scheduler.Schedule(&command); + EXPECT_EQ(1, initCounter); + EXPECT_EQ(0, exeCounter); + EXPECT_EQ(0, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + isFinishedHook = false; + scheduler.Run(); + EXPECT_EQ(1, initCounter); + EXPECT_EQ(1, exeCounter); + EXPECT_EQ(1, isFinishedCounter); + EXPECT_EQ(0, endCounter); + + isFinishedHook = true; + scheduler.Run(); + EXPECT_EQ(2, initCounter); + EXPECT_EQ(2, exeCounter); + EXPECT_EQ(2, isFinishedCounter); + EXPECT_EQ(1, endCounter); + + isFinishedHook = false; + scheduler.Run(); + EXPECT_EQ(2, initCounter); + EXPECT_EQ(3, exeCounter); + EXPECT_EQ(3, isFinishedCounter); + EXPECT_EQ(1, endCounter); +}