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