From 517b6e968e402f1b8afbca98f2d16f26e27a9692 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 13 Aug 2024 22:27:46 -0700 Subject: [PATCH] Clean up unit UDL usage --- .../main/native/cpp/analysis/AnalysisManager.cpp | 2 +- .../frc2/command/TrapezoidProfileCommand.h | 7 ++----- .../cpp/frc2/command/NotifierCommandTest.cpp | 4 ++-- wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 2 +- .../main/native/cpp/PneumaticsControlModule.cpp | 2 +- .../src/main/native/cpp/counter/Tachometer.cpp | 6 +++--- wpilibc/src/main/native/include/frc/LEDPattern.h | 2 +- .../native/cpp/simulation/EncoderSimTest.cpp | 2 +- .../FlywheelBangBangController/cpp/Robot.cpp | 2 +- .../RapidReactCommandBot/include/Constants.h | 16 +++++++--------- .../examples/rapidreactcommandbot/Constants.java | 10 ++++------ ...ontrolAffinePlantInversionFeedforwardTest.cpp | 4 ++-- .../controller/HolonomicDriveControllerTest.cpp | 2 +- .../LTVDifferentialDriveControllerTest.cpp | 2 +- .../cpp/controller/LTVUnicycleControllerTest.cpp | 2 +- .../controller/LinearQuadraticRegulatorTest.cpp | 4 ++-- .../cpp/controller/RamseteControllerTest.cpp | 2 +- .../DifferentialDrivePoseEstimatorTest.cpp | 10 +++++----- .../estimator/MecanumDrivePoseEstimatorTest.cpp | 10 +++++----- .../estimator/SwerveDrivePoseEstimatorTest.cpp | 8 ++++---- .../cpp/kinematics/MecanumDriveOdometryTest.cpp | 4 ++-- .../cpp/kinematics/SwerveDriveOdometryTest.cpp | 4 ++-- .../cpp/system/plant/proto/DCMotorProtoTest.cpp | 11 ++--------- .../system/plant/struct/DCMotorStructTest.cpp | 12 +++--------- .../cpp/trajectory/ExponentialProfileTest.cpp | 4 ++-- 25 files changed, 57 insertions(+), 77 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp index 52121ebdda4..b76b4414e5c 100644 --- a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp +++ b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp @@ -172,7 +172,7 @@ AnalysisManager::AnalysisManager(TestData data, Settings& settings, wpi::Logger& logger) : m_data{std::move(data)}, m_logger{logger}, m_settings{settings} { // Reset settings for Dynamic Test Limits - m_settings.stepTestDuration = units::second_t{0.0}; + m_settings.stepTestDuration = 0_s; m_settings.velocityThreshold = std::numeric_limits::infinity(); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index 096402a599f..7b1d70214d4 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -56,15 +56,12 @@ class TrapezoidProfileCommand void Initialize() override {} void Execute() override { - m_output( - m_profile.Calculate(units::second_t{0.02}, m_currentState(), m_goal())); + m_output(m_profile.Calculate(20_ms, m_currentState(), m_goal())); } void End(bool interrupted) override {} - bool IsFinished() override { - return m_profile.IsFinished(units::second_t{0}); - } + bool IsFinished() override { return m_profile.IsFinished(0_s); } private: frc::TrapezoidProfile m_profile; diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp index 00c9b89ea84..8b4f1cc909b 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp @@ -18,11 +18,11 @@ TEST_F(NotifierCommandTest, NotifierCommandSchedule) { frc::sim::PauseTiming(); int counter = 0; - NotifierCommand command([&] { counter++; }, 0.01_s, {}); + NotifierCommand command([&] { counter++; }, 10_ms, {}); scheduler.Schedule(&command); for (int i = 0; i < 5; ++i) { - frc::sim::StepTiming(0.005_s); + frc::sim::StepTiming(5_ms); } scheduler.Cancel(&command); diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 5af75b8a6d1..b29c498ad17 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -818,7 +818,7 @@ units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const { break; } - return units::degree_t{0.0}; + return 0_deg; } units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp index 60cd84dcb32..e34e85097d2 100644 --- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp +++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp @@ -264,7 +264,7 @@ void PneumaticsControlModule::UnreserveCompressor() { } units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const { - return units::volt_t{0}; + return 0_V; } units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure( diff --git a/wpilibc/src/main/native/cpp/counter/Tachometer.cpp b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp index 90688d2c386..42dd22a3e15 100644 --- a/wpilibc/src/main/native/cpp/counter/Tachometer.cpp +++ b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp @@ -45,7 +45,7 @@ Tachometer::~Tachometer() { units::hertz_t Tachometer::GetFrequency() const { auto period = GetPeriod(); if (period.value() == 0) { - return units::hertz_t{0.0}; + return 0_Hz; } return 1 / period; } @@ -67,11 +67,11 @@ void Tachometer::SetEdgesPerRevolution(int edges) { units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const { auto period = GetPeriod(); if (period.value() == 0) { - return units::turns_per_second_t{0.0}; + return 0_tps; } int edgesPerRevolution = GetEdgesPerRevolution(); if (edgesPerRevolution == 0) { - return units::turns_per_second_t{0.0}; + return 0_tps; } auto rotationHz = ((1.0 / edgesPerRevolution) / period); return units::turns_per_second_t{rotationHz.value()}; diff --git a/wpilibc/src/main/native/include/frc/LEDPattern.h b/wpilibc/src/main/native/include/frc/LEDPattern.h index 37e743d5ba9..43b1614e2ab 100644 --- a/wpilibc/src/main/native/include/frc/LEDPattern.h +++ b/wpilibc/src/main/native/include/frc/LEDPattern.h @@ -112,7 +112,7 @@ class LEDPattern { * * frc::LEDPattern rainbow = frc::LEDPattern::Rainbow(); * frc::LEDPattern scrollingRainbow = - * rainbow.ScrollAtAbsoluteSpeed(units::inches_per_second_t{4}, + * rainbow.ScrollAtAbsoluteSpeed(units::feet_per_second_t{1 / 3.0}, * LED_SPACING); * * diff --git a/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp index 2a763af2920..7d35e7cf9ed 100644 --- a/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp @@ -114,7 +114,7 @@ TEST(EncoderSimTest, SetMaxPeriod) { auto cb = sim.RegisterMaxPeriodCallback(callback.GetCallback(), false); WPI_IGNORE_DEPRECATED - encoder.SetMaxPeriod(units::second_t{123.456}); + encoder.SetMaxPeriod(123.456_s); WPI_UNIGNORE_DEPRECATED EXPECT_EQ(123.456, sim.GetMaxPeriod()); diff --git a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp index 88e552198a5..fd5b2a2e37f 100644 --- a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp @@ -54,7 +54,7 @@ class Robot : public frc::TimedRobot { m_flywheelSim.SetInputVoltage( m_flywheelMotor.Get() * units::volt_t{frc::RobotController::GetInputVoltage()}); - m_flywheelSim.Update(0.02_s); + m_flywheelSim.Update(20_ms); m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value()); } diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h index a552c448560..91e59d1f29e 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h @@ -24,7 +24,7 @@ inline constexpr bool kLeftEncoderReversed = false; inline constexpr bool kRightEncoderReversed = true; inline constexpr double kEncoderCPR = 1024; -inline constexpr units::meter_t kWheelDiameter = 6.0_in; +inline constexpr units::meter_t kWheelDiameter = 6_in; inline constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value(); @@ -45,9 +45,8 @@ namespace ShooterConstants { inline constexpr int kEncoderPorts[]{4, 5}; inline constexpr bool kEncoderReversed = false; inline constexpr double kEncoderCPR = 1024; -inline constexpr double kEncoderDistancePerPulse = - // Distance units will be rotations - 1.0 / kEncoderCPR; +// Distance units will be rotations +inline constexpr double kEncoderDistancePerPulse = 1.0 / kEncoderCPR; inline constexpr int kShooterMotorPort = 4; inline constexpr int kFeederMotorPort = 5; @@ -61,9 +60,8 @@ inline constexpr auto kShooterTolerance = 50_tps; inline constexpr double kP = 1; inline constexpr units::volt_t kS = 0.05_V; -inline constexpr auto kV = - // Should have value 12V at free speed... - 12.0_V / kShooterFree; +// Should have value 12V at free speed +inline constexpr auto kV = 12_V / kShooterFree; inline constexpr double kFeederSpeed = 0.5; } // namespace ShooterConstants @@ -73,7 +71,7 @@ inline constexpr int kDriverControllerPort = 0; } // namespace OIConstants namespace AutoConstants { -constexpr units::second_t kTimeout = 3.0_s; -constexpr units::meter_t kDriveDistance = 2.0_m; +constexpr units::second_t kTimeout = 3_s; +constexpr units::meter_t kDriveDistance = 2_m; constexpr double kDriveSpeed = 0.5; } // namespace AutoConstants diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java index 31ce798c65f..17d025035fc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java @@ -37,9 +37,8 @@ public static final class ShooterConstants { public static final int[] kEncoderPorts = {4, 5}; public static final boolean kEncoderReversed = false; public static final int kEncoderCPR = 1024; - public static final double kEncoderDistancePerPulse = - // Distance units will be rotations - 1.0 / kEncoderCPR; + // Distance units will be rotations + public static final double kEncoderDistancePerPulse = 1.0 / kEncoderCPR; public static final int kShooterMotorPort = 4; public static final int kFeederMotorPort = 5; @@ -54,9 +53,8 @@ public static final class ShooterConstants { // On a real robot the feedforward constants should be empirically determined; these are // reasonable guesses. public static final double kSVolts = 0.05; - public static final double kVVoltSecondsPerRotation = - // Should have value 12V at free speed... - 12.0 / kShooterFreeRPS; + // Should have value 12V at free speed + public static final double kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS; public static final double kFeederSpeed = 0.5; } diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp index 9db64daea13..5c8e3f15fab 100644 --- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp @@ -25,8 +25,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) { std::function(const Vectord<2>&, const Vectord<1>&)> modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); }; - frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ - modelDynamics, units::second_t{0.02}}; + frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics, + 20_ms}; Vectord<2> r{2, 2}; Vectord<2> nextR{3, 3}; diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp index 4b4a883eaa6..746a7d08af6 100644 --- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp @@ -36,7 +36,7 @@ TEST(HolonomicDriveControllerTest, ReachesReference) { auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( waypoints, {8.0_mps, 4.0_mps_sq}); - constexpr auto kDt = 0.02_s; + constexpr units::second_t kDt = 20_ms; auto totalTime = trajectory.TotalTime(); for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { auto state = trajectory.Sample(kDt * i); diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp index d96c67b4f27..f602c702448 100644 --- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp @@ -62,7 +62,7 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { } TEST(LTVDifferentialDriveControllerTest, ReachesReference) { - constexpr auto kDt = 0.02_s; + constexpr units::second_t kDt = 20_ms; frc::LTVDifferentialDriveController controller{ plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt}; diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp index ff2c7d31864..93b4e74c427 100644 --- a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp @@ -17,7 +17,7 @@ static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi / 180.0}; TEST(LTVUnicycleControllerTest, ReachesReference) { - constexpr auto kDt = 0.02_s; + constexpr units::second_t kDt = 20_ms; frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt}; frc::Pose2d robotPose{2.7_m, 23_m, 0_deg}; diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index ab7004ef721..3efb08d6141 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -180,9 +180,9 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); }(); - LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s}; + LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms}; - controller.LatencyCompensate(plant, 0.02_s, 0.01_s); + controller.LatencyCompensate(plant, 20_ms, 10_ms); EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3); EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3); diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp index 6794b9cb02f..8c39cac31ba 100644 --- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -29,7 +29,7 @@ TEST(RamseteControllerTest, ReachesReference) { auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( waypoints, {8.8_mps, 0.1_mps_sq}); - constexpr auto kDt = 0.02_s; + constexpr units::second_t kDt = 20_ms; auto totalTime = trajectory.TotalTime(); for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { auto state = trajectory.Sample(kDt * i); diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index feffeaf2659..3fb42f9c9c2 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -167,8 +167,8 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) { state.velocity * state.curvature}; }, [&](frc::Trajectory::State& state) { return state.pose; }, - trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, - 0.1_s, 0.25_s, true, false); + trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, + 100_ms, 250_ms, true, false); } TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) { @@ -205,8 +205,8 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) { state.velocity * state.curvature}; }, [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); } } } @@ -275,7 +275,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer - for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { + for (auto time = 0_s; time < 4_s; time += 20_ms) { estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m); } diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index f4134c9c3c7..1132866b778 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -162,8 +162,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) { state.velocity * state.curvature}; }, [&](frc::Trajectory::State& state) { return state.pose; }, - trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, - 0.1_s, 0.25_s, true, false); + trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, + 100_ms, 250_ms, true, false); } TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) { @@ -204,8 +204,8 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) { state.velocity * state.curvature}; }, [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); } } } @@ -276,7 +276,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer - for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { + for (auto time = 0_s; time < 4_s; time += 20_ms) { estimator.UpdateWithTime(time, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}); } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 52c4d6432fb..6296ee36d10 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -169,7 +169,7 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) { }, [&](frc::Trajectory::State& state) { return state.pose; }, {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}}, - 0.02_s, 0.1_s, 0.25_s, true, false); + 20_ms, 100_ms, 250_ms, true, false); } TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) { @@ -213,8 +213,8 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) { state.velocity * state.curvature}; }, [&](frc::Trajectory::State& state) { return state.pose; }, - initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s, - 0.25_s, false, false); + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); } } } @@ -293,7 +293,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer - for (auto time = 0.0_s; time < 4_s; time += 0.02_s) { + for (auto time = 0_s; time < 4_s; time += 20_ms) { estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br}); } diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index be206553b46..4e48726449b 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -97,7 +97,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) { std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); - units::second_t dt = 0.02_s; + units::second_t dt = 20_ms; units::second_t t = 0_s; double maxError = -std::numeric_limits::max(); @@ -160,7 +160,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) { std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); - units::second_t dt = 0.02_s; + units::second_t dt = 20_ms; units::second_t t = 0_s; double maxError = -std::numeric_limits::max(); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index 265b0cc6c3f..9595335071b 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -93,7 +93,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) { std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); - units::second_t dt = 0.02_s; + units::second_t dt = 20_ms; units::second_t t = 0_s; double maxError = -std::numeric_limits::max(); @@ -157,7 +157,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) { std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); - units::second_t dt = 0.02_s; + units::second_t dt = 20_ms; units::second_t t = 0_s; double maxError = -std::numeric_limits::max(); diff --git a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp index b664befd762..d977cf4e715 100644 --- a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp +++ b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp @@ -9,17 +9,10 @@ using namespace frc; -namespace { - using ProtoType = wpi::Protobuf; -const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, - units::newton_meter_t{19.1}, - units::ampere_t{1.74}, - units::ampere_t{2.29}, - units::radians_per_second_t{2.2}, - 2}; -} // namespace +inline constexpr DCMotor kExpectedData = + DCMotor{1.91_V, 19.1_Nm, 1.74_A, 2.29_A, 2.2_rad_per_s, 2}; TEST(DCMotorProtoTest, Roundtrip) { google::protobuf::Arena arena; diff --git a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp index 50d5c8b5607..e7e229b90ef 100644 --- a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp +++ b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp @@ -8,16 +8,10 @@ using namespace frc; -namespace { - using StructType = wpi::Struct; -const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, - units::newton_meter_t{19.1}, - units::ampere_t{1.74}, - units::ampere_t{2.29}, - units::radians_per_second_t{2.2}, - 2}; -} // namespace + +inline constexpr DCMotor kExpectedData = + DCMotor{1.91_V, 19.1_Nm, 1.74_A, 2.29_A, 2.2_rad_per_s, 2}; TEST(DCMotorStructTest, Roundtrip) { uint8_t buffer[StructType::GetSize()]; diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp index 663ed3af09d..e4ecb518730 100644 --- a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp @@ -306,7 +306,7 @@ TEST(ExponentialProfileTest, TimingToGoal) { for (int i = 0; i < 900; ++i) { state = CheckDynamics(profile, constraints, feedforward, state, goal); if (!reachedGoal && state == goal) { - EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s); + EXPECT_NEAR_UNITS(prediction, i * 10_ms, 250_ms); reachedGoal = true; } } @@ -329,7 +329,7 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) { for (int i = 0; i < 900; ++i) { state = CheckDynamics(profile, constraints, feedforward, state, goal); if (!reachedGoal && state == goal) { - EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s); + EXPECT_NEAR_UNITS(prediction, i * 10_ms, 250_ms); reachedGoal = true; } }