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);