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