[command] Speed up MecanumControllerCommand and SwerveControllerCommand tests (#2604)

Currently, these two tests take several seconds to complete and fail
intermittently in Windows CI. This is caused by relying on wall clock
time.

Sampling the trajectory with wall clock time means the simulation must
run for several seconds to reach the end of the trajectory. Also, the
controller can become unstable when Windows CI experiences process
scheduling delays of several hundred milliseconds. Feedback controllers
don't cope well with large delays on systems with fast dynamics.

This patch uses the mocking functionality of frc::Timer to advance the
clock by 5ms at every timestep instead of using the wall clock time.
This has two benefits:

1. The tests complete much faster because the simulation can step
   forward faster than real time.
2. The controller is more stable because the sample period is uniform,
   which should fix the occasional failures.

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

@@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command;
 
 import java.util.ArrayList;
 
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.Timer;
@@ -20,6 +22,7 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -28,6 +31,16 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class MecanumControllerCommandTest {
+  @BeforeAll
+  static void setupAll() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterAll
+  static void cleanupAll() {
+    SimHooks.resumeTiming();
+  }
+
   private final Timer m_timer = new Timer();
   private Rotation2d m_angle = new Rotation2d(0);
 
@@ -102,6 +115,7 @@ class MecanumControllerCommandTest {
     while (!command.isFinished()) {
       command.execute();
       m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+      SimHooks.stepTiming(0.005);
     }
     m_timer.stop();
     command.end(true);

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

@@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command;
 
 import java.util.ArrayList;
 
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.Timer;
@@ -20,6 +22,7 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -28,6 +31,16 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class SwerveControllerCommandTest {
+  @BeforeAll
+  static void setupAll() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterAll
+  static void cleanupAll() {
+    SimHooks.resumeTiming();
+  }
+
   private final Timer m_timer = new Timer();
   private Rotation2d m_angle = new Rotation2d(0);
 
@@ -95,6 +108,7 @@ class SwerveControllerCommandTest {
     while (!command.isFinished()) {
       command.execute();
       m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+      SimHooks.stepTiming(0.005);
     }
     m_timer.stop();
     command.end(true);

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

@@ -9,14 +9,13 @@
 #include <frc2/command/MecanumControllerCommand.h>
 #include <frc2/command/Subsystem.h>
 
-#include <iostream>
-
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <wpi/math>
 
@@ -60,6 +59,10 @@ class MecanumControllerCommandTest : public ::testing::Test {
   frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
                                        frc::Pose2d{0_m, 0_m, 0_rad}};
 
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+
   frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
     return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
                                         m_rearLeftSpeed, m_rearRightSpeed};
@@ -72,7 +75,7 @@ class MecanumControllerCommandTest : public ::testing::Test {
 };
 
 TEST_F(MecanumControllerCommandTest, ReachesReference) {
-  frc2::Subsystem subsystem{};
+  frc2::Subsystem subsystem;
 
   auto waypoints =
       std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
@@ -99,10 +102,12 @@ TEST_F(MecanumControllerCommandTest, ReachesReference) {
 
   m_timer.Reset();
   m_timer.Start();
+
   command.Initialize();
   while (!command.IsFinished()) {
     command.Execute();
     m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+    frc::sim::StepTiming(5_ms);
   }
   m_timer.Stop();
   command.End(false);

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

@@ -9,8 +9,6 @@
 #include <frc2/command/Subsystem.h>
 #include <frc2/command/SwerveControllerCommand.h>
 
-#include <iostream>
-
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
@@ -18,6 +16,7 @@
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/kinematics/SwerveDriveOdometry.h>
 #include <frc/kinematics/SwerveModuleState.h>
+#include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <wpi/math>
 
@@ -60,6 +59,10 @@ class SwerveControllerCommandTest : public ::testing::Test {
   frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
                                          frc::Pose2d{0_m, 0_m, 0_rad}};
 
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+
   std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
     return m_moduleStates;
   }
@@ -71,7 +74,7 @@ class SwerveControllerCommandTest : public ::testing::Test {
 };
 
 TEST_F(SwerveControllerCommandTest, ReachesReference) {
-  frc2::Subsystem subsystem{};
+  frc2::Subsystem subsystem;
 
   auto waypoints =
       std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
@@ -89,10 +92,12 @@ TEST_F(SwerveControllerCommandTest, ReachesReference) {
 
   m_timer.Reset();
   m_timer.Start();
+
   command.Initialize();
   while (!command.IsFinished()) {
     command.Execute();
     m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+    frc::sim::StepTiming(5_ms);
   }
   m_timer.Stop();
   command.End(false);