[commands] Add RepeatCommand (#4009)

Co-authored-by: Starlight220 <53231611+Starlight220@users.noreply.github.com>

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());
+  }
+}

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