Add command.repeatdly(int count) to both java and c++

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -15,6 +15,7 @@
 import java.util.Collection;
 import java.util.HashSet;
 import java.util.Set;
+import java.util.concurrent.atomic.AtomicInteger;
 import java.util.function.BooleanSupplier;
 
 /**
@@ -382,6 +383,30 @@ public RepeatCommand repeatedly() {
     return new RepeatCommand(this);
   }
 
+  /**
+   * Decorates this command to run repeatedly, restarting until the command runs for the given times
+   * amount. The decorated command can still be canceled.
+   *
+   * <p>This is just syntactic sugar for {@code this.finallyDo(() ->
+   * counter[0]++).repeatedly().until(() -> counter[0] >= times)} which just means: runs the
+   * command, then increments the counter, repeatedly until the counter saturates.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param times the count of times to run the command (inclusively).
+   * @return the decorated command
+   */
+  public ParallelRaceGroup repeatedly(int times) {
+    AtomicInteger counter = new AtomicInteger(0);
+    return this.finallyDo(counter::getAndIncrement)
+        .repeatedly()
+        .until(() -> counter.get() >= times);
+  }
+
   /**
    * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. Use this for
    * "forking off" from command compositions when the user does not wish to extend the command's

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -109,6 +109,10 @@ CommandPtr Command::Repeatedly() && {
   return std::move(*this).ToPtr().Repeatedly();
 }
 
+CommandPtr Command::Repeatedly(int times) && {
+  return std::move(*this).ToPtr().Repeatedly(times);
+}
+
 CommandPtr Command::AsProxy() && {
   return std::move(*this).ToPtr().AsProxy();
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -50,6 +50,17 @@ CommandPtr CommandPtr::Repeatedly() && {
   return std::move(*this);
 }
 
+CommandPtr CommandPtr::Repeatedly(int times) && {
+  AssertValid();
+  std::shared_ptr<int> countPtr = std::make_shared<int>(0);
+  return std::move(*this)
+      .FinallyDo([countPtr = countPtr] { (*countPtr)++; })
+      .Repeatedly()
+      .Until([countPtr = countPtr, times = times] {
+        return ((*countPtr) >= times);
+      });
+}
+
 CommandPtr CommandPtr::AsProxy() && {
   AssertValid();
   m_ptr = std::make_unique<ProxyCommand>(std::move(m_ptr));

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -269,6 +269,16 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
   [[nodiscard]]
   CommandPtr Repeatedly() &&;
 
+  /**
+   * Decorates this command to run repeatedly until the given count is reached
+   * or is interrupted. The decorated command can still be canceled.
+   *
+   * @param times the number/count of times to run the command
+   * @return the decorated command
+   */
+  [[nodiscard]]
+  CommandPtr Repeatedly(int times) &&;
+
   /**
    * Decorates this command to run "by proxy" by wrapping it in a ProxyCommand.
    * Use this for "forking off" from command compositions when the user does not

wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h

@@ -49,6 +49,16 @@ class CommandPtr final {
   [[nodiscard]]
   CommandPtr Repeatedly() &&;
 
+  /**
+   * Decorates this command to run repeatedly until the given count is reached
+   * or is interrupted. The decorated command can still be canceled.
+   *
+   * @param times the number of times to run the command
+   * @return the decorated command
+   */
+  [[nodiscard]]
+  CommandPtr Repeatedly(int times) &&;
+
   /**
    * Decorates this command to run "by proxy" by wrapping it in a ProxyCommand.
    * Use this for "forking off" from command compositions when the user does not

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -369,6 +369,46 @@ void raceWithOrderTest() {
     }
   }
 
+  @Test
+  void repeatedlyTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger(0);
+
+      Command command = new InstantCommand(counter::incrementAndGet);
+
+      Command group = command.repeatedly();
+
+      scheduler.schedule(group);
+      for (int i = 1; i <= 50; i++) {
+        scheduler.run();
+        assertEquals(i, counter.get());
+      }
+
+      // Should still be scheduled
+      assertTrue(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void repeatForTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger(0);
+
+      Command command = new InstantCommand(counter::incrementAndGet);
+
+      Command group = command.repeatedly(3);
+
+      scheduler.schedule(group);
+      for (int i = 0;
+          scheduler.isScheduled(group);
+          i++) { // If this causes an infinite loop, repeatedly is cooked
+        scheduler.run();
+        assertEquals(i + 1, counter.get());
+      }
+      assertEquals(3, counter.get());
+    }
+  }
+
   @Test
   void unlessTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -335,6 +335,39 @@ TEST_F(CommandDecoratorTest, RaceWithOrder) {
   EXPECT_TRUE(firstWasPolled);
 }
 
+TEST_F(CommandDecoratorTest, Repeatedly) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command = InstantCommand([&counter] { counter++; }, {}).Repeatedly();
+
+  scheduler.Schedule(command);
+
+  for (int i = 1; i <= 50; i++) {
+    scheduler.Run();
+    EXPECT_EQ(i, counter);
+  }
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, RepeatFor) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command = InstantCommand([&counter] { counter++; }, {}).Repeatedly(3);
+
+  scheduler.Schedule(command);
+  for (int i = 0; scheduler.IsScheduled(command); i++) {
+    scheduler.Run();
+    EXPECT_EQ(i + 1, counter);
+  }
+
+  EXPECT_EQ(3, counter);
+}
+
 TEST_F(CommandDecoratorTest, Unless) {
   CommandScheduler scheduler = GetScheduler();