From 49dcf7cf59272653ff2797a5df2cce4e7d13fb8e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 18 Jul 2020 21:58:37 -0700 Subject: [PATCH] [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. --- .../command/MecanumControllerCommandTest.java | 14 ++++++++++++++ .../command/SwerveControllerCommandTest.java | 14 ++++++++++++++ .../frc2/command/MecanumControllerCommandTest.cpp | 11 ++++++++--- .../frc2/command/SwerveControllerCommandTest.cpp | 11 ++++++++--- 4 files changed, 44 insertions(+), 6 deletions(-) diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java index 9467c62883..92ecf694a9 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ b/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); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index fcddd5eba2..56488ca0f5 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/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); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp index abfc84bee8..07bc07a1d9 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -9,14 +9,13 @@ #include #include -#include - #include #include #include #include #include #include +#include #include #include @@ -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); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index 83ba26f979..b2c47d04d9 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -9,8 +9,6 @@ #include #include -#include - #include #include #include @@ -18,6 +16,7 @@ #include #include #include +#include #include #include @@ -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 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);