Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace .to<double>() and .template to<double>() with .value() (#3667)

Browse Source 
It's a less verbose way to do the same thing.
This commit is contained in:
Tyler Veness
2021-10-25 08:58:12 -07:00
committed by GitHub
parent 6bc1db44bc
commit 181723e573
134 changed files with 782 additions and 826 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -11,7 +11,7 @@ TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}

32
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
View File

@@ -20,8 +20,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
@@ -29,9 +29,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
@@ -39,8 +39,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
@@ -48,9 +48,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
@@ -59,10 +59,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
@@ -72,7 +70,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
}

6
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -18,7 +18,7 @@ TEST(DifferentialDriveOdometryTest, EncoderDistances) {
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::numbers::pi));
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon);
}

92
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -25,38 +25,38 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(5.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
@@ -64,10 +64,10 @@ TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
@@ -75,19 +75,19 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
-150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
@@ -96,19 +96,19 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
@@ -116,9 +116,9 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -126,10 +126,10 @@ TEST_F(MecanumDriveKinematicsTest,
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
EXPECT_NEAR(3.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -138,9 +138,9 @@ TEST_F(MecanumDriveKinematicsTest,
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(12.02, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, Normalize) {
@@ -149,8 +149,8 @@ TEST_F(MecanumDriveKinematicsTest, Normalize) {
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
}

24
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -26,9 +26,9 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
@@ -38,9 +38,9 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
@@ -50,9 +50,9 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}

118
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -28,15 +28,15 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(br.angle.Radians().value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
@@ -44,33 +44,33 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), 90.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
@@ -78,15 +78,15 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
@@ -97,9 +97,9 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
@@ -107,15 +107,15 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
units::radians_per_second_t(2 * wpi::numbers::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
@@ -126,9 +126,9 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -137,15 +137,15 @@ TEST_F(SwerveDriveKinematicsTest,
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -109.44, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -70.56, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -157,9 +157,9 @@ TEST_F(SwerveDriveKinematicsTest,
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, Normalize) {
@@ -173,8 +173,8 @@ TEST_F(SwerveDriveKinematicsTest, Normalize) {
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -31,9 +31,9 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
@@ -49,9 +49,9 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
@@ -65,7 +65,7 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}

16
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
View File

@@ -13,15 +13,15 @@ TEST(SwerveModuleStateTest, Optimize) {
frc::SwerveModuleState refA{-2_mps, 180_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
frc::Rotation2d angleB{-50_deg};
frc::SwerveModuleState refB{4.7_mps, 41_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -139.0, kEpsilon);
EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleStateTest, NoOptimize) {
@@ -29,13 +29,13 @@ TEST(SwerveModuleStateTest, NoOptimize) {
frc::SwerveModuleState refA{2_mps, 89_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 89.0, kEpsilon);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
frc::Rotation2d angleB{0_deg};
frc::SwerveModuleState refB{-2_mps, -2_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
}