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

SCRIPT namespace replacements

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,15 +11,15 @@
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, Discretize) {
constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr units::second_t duration = 1_s;
constexpr units::second_t dt = 10_ms;
constexpr wpi::math::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto speeds = target.Discretize(duration);
const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
frc::Pose2d pose;
for (units::second_t time = 0_s; time < duration; time += dt) {
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
@@ -31,7 +31,7 @@ TEST(ChassisSpeedsTest, Discretize) {
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
@@ -41,7 +41,7 @@ TEST(ChassisSpeedsTest, ToRobotRelative) {
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
@@ -49,10 +49,10 @@ TEST(ChassisSpeedsTest, ToFieldRelative) {
}
TEST(ChassisSpeedsTest, Plus) {
const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const frc::ChassisSpeeds result = left + right;
const wpi::math::ChassisSpeeds result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
@@ -60,10 +60,10 @@ TEST(ChassisSpeedsTest, Plus) {
}
TEST(ChassisSpeedsTest, Minus) {
const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const frc::ChassisSpeeds result = left - right;
const wpi::math::ChassisSpeeds result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
@@ -71,9 +71,9 @@ TEST(ChassisSpeedsTest, Minus) {
}
TEST(ChassisSpeedsTest, UnaryMinus) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = -speeds;
const wpi::math::ChassisSpeeds result = -speeds;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
@@ -81,9 +81,9 @@ TEST(ChassisSpeedsTest, UnaryMinus) {
}
TEST(ChassisSpeedsTest, Multiplication) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = speeds * 2;
const wpi::math::ChassisSpeeds result = speeds * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
@@ -91,9 +91,9 @@ TEST(ChassisSpeedsTest, Multiplication) {
}
TEST(ChassisSpeedsTest, Division) {
const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds result = speeds / 2;
const wpi::math::ChassisSpeeds result = speeds / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);

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

@@ -12,7 +12,7 @@
#include "wpi/units/length.hpp"
#include "wpi/units/velocity.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 1E-9;
@@ -57,7 +57,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{
0.0_mps, 0.0_mps, units::radians_per_second_t{std::numbers::pi}};
0.0_mps, 0.0_mps, wpi::units::radians_per_second_t{std::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
@@ -67,8 +67,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
units::meters_per_second_t{+0.381 * std::numbers::pi},
units::meters_per_second_t{-0.381 * std::numbers::pi}};
wpi::units::meters_per_second_t{+0.381 * std::numbers::pi},
wpi::units::meters_per_second_t{-0.381 * std::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);

14
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp
View File

@@ -11,14 +11,14 @@
static constexpr double kEpsilon = 1E-9;
using namespace frc;
using namespace wpi::math;
TEST(DifferentialDriveOdometry3dTest, Initialize) {
DifferentialDriveOdometry3d odometry{
frc::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
const wpi::math::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
@@ -27,11 +27,11 @@ TEST(DifferentialDriveOdometry3dTest, Initialize) {
}
TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg},
DifferentialDriveOdometry3d odometry{wpi::math::Rotation3d{0_deg, 0_deg, 45_deg},
0_m, 0_m};
const auto& pose = odometry.Update(frc::Rotation3d{0_deg, 0_deg, 135_deg},
0_m, units::meter_t{5 * std::numbers::pi});
const auto& pose = odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 135_deg},
0_m, wpi::units::meter_t{5 * std::numbers::pi});
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);

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

@@ -11,13 +11,13 @@
static constexpr double kEpsilon = 1E-9;
using namespace frc;
using namespace wpi::math;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
const auto& pose =
odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});
odometry.Update(135_deg, 0_m, wpi::units::meter_t{5 * std::numbers::pi});
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);

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

@@ -7,47 +7,47 @@
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
TEST(DifferentialDriveWheelSpeedsTest, Plus) {
const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
const frc::DifferentialDriveWheelSpeeds result = left + right;
const wpi::math::DifferentialDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.left.value());
EXPECT_EQ(2.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Minus) {
const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds result = left - right;
const wpi::math::DifferentialDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds result = -speeds;
const wpi::math::DifferentialDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(-0.5, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds result = speeds * 2;
const wpi::math::DifferentialDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.left.value());
EXPECT_EQ(1.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Division) {
const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const frc::DifferentialDriveWheelSpeeds result = speeds / 2;
const wpi::math::DifferentialDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.left.value());
EXPECT_EQ(0.25, result.right.value());

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

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/units/angular_velocity.hpp"
using namespace frc;
using namespace wpi::math;
class MecanumDriveKinematicsTest : public ::testing::Test {
protected:
@@ -80,7 +80,7 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t{2 * std::numbers::pi}};
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);

94
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/MecanumDriveOdometry3d.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
using namespace frc;
using namespace wpi::math;
class MecanumDriveOdometry3dTest : public ::testing::Test {
protected:
@@ -22,15 +22,15 @@ class MecanumDriveOdometry3dTest : public ::testing::Test {
MecanumDriveWheelPositions zero;
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero};
MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{}, zero};
};
TEST_F(MecanumDriveOdometry3dTest, Initialize) {
MecanumDriveOdometry3d odometry{
kinematics, frc::Rotation3d{}, zero,
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
kinematics, wpi::math::Rotation3d{}, zero,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
const wpi::math::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, 1e-9);
EXPECT_NEAR(pose.Y().value(), 2, 1e-9);
@@ -41,10 +41,10 @@ TEST_F(MecanumDriveOdometry3dTest, Initialize) {
TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.ResetPosition(frc::Rotation3d{}, wheelDeltas, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, wheelDeltas, Pose3d{});
odometry.Update(frc::Rotation3d{}, wheelDeltas);
auto secondPose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
auto secondPose = odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -54,12 +54,12 @@ TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose = odometry.Update(frc::Rotation3d{}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose = odometry.Update(wpi::math::Rotation3d{}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -68,12 +68,12 @@ TEST_F(MecanumDriveOdometry3dTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{});
odometry.Update(wpi::math::Rotation3d{}, MecanumDriveWheelPositions{});
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -82,13 +82,13 @@ TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
odometry.ResetPosition(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
auto pose =
odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -97,33 +97,33 @@ TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
}
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity, 0_mps,
@@ -140,8 +140,8 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad}},
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
@@ -160,33 +160,33 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
}
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{},
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
@@ -204,7 +204,7 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wpi::math::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())

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

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/MecanumDriveOdometry.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
using namespace frc;
using namespace wpi::math;
class MecanumDriveOdometryTest : public ::testing::Test {
protected:
@@ -78,33 +78,33 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
}
TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity, 0_mps,
@@ -122,7 +122,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -141,33 +141,33 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
}
TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveKinematics kinematics{
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
frc::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
wheelPositions};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 135_deg},
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
wpi::math::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
@@ -185,7 +185,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();

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

@@ -7,10 +7,10 @@
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
TEST(MecanumDriveWheelSpeedsTest, Plus) {
const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
const frc::MecanumDriveWheelSpeeds result = left + right;
const wpi::math::MecanumDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.frontLeft.value());
EXPECT_EQ(2.0, result.frontRight.value());
@@ -19,10 +19,10 @@ TEST(MecanumDriveWheelSpeedsTest, Plus) {
}
TEST(MecanumDriveWheelSpeedsTest, Minus) {
const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
const frc::MecanumDriveWheelSpeeds result = left - right;
const wpi::math::MecanumDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-1.0, result.frontRight.value());
@@ -31,9 +31,9 @@ TEST(MecanumDriveWheelSpeedsTest, Minus) {
}
TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const frc::MecanumDriveWheelSpeeds result = -speeds;
const wpi::math::MecanumDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-0.5, result.frontRight.value());
@@ -42,9 +42,9 @@ TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
}
TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const frc::MecanumDriveWheelSpeeds result = speeds * 2;
const wpi::math::MecanumDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.frontLeft.value());
EXPECT_EQ(1.0, result.frontRight.value());
@@ -53,9 +53,9 @@ TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
}
TEST(MecanumDriveWheelSpeedsTest, Division) {
const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
const frc::MecanumDriveWheelSpeeds result = speeds / 2;
const wpi::math::MecanumDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.frontLeft.value());
EXPECT_EQ(0.25, result.frontRight.value());

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

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/units/angular_velocity.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 0.1;
@@ -96,7 +96,7 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t{2 * std::numbers::pi}};
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
@@ -112,7 +112,7 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t{2 * std::numbers::pi}};
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
@@ -174,7 +174,7 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t{2 * std::numbers::pi}};
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
@@ -266,7 +266,7 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
@@ -283,7 +283,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
&arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
@@ -301,7 +301,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
SwerveModuleState state3{-2.0_mps, 0_deg};
SwerveModuleState state4{-2.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);

42
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
View File

@@ -13,7 +13,7 @@
#include "wpi/math/trajectory/TrajectoryConfig.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 0.01;
@@ -27,17 +27,17 @@ class SwerveDriveOdometry3dTest : public ::testing::Test {
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
SwerveModulePosition zero;
SwerveDriveOdometry3d<4> m_odometry{
m_kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
m_kinematics, wpi::math::Rotation3d{}, {zero, zero, zero, zero}};
};
TEST_F(SwerveDriveOdometry3dTest, Initialize) {
SwerveDriveOdometry3d odometry{
m_kinematics,
frc::Rotation3d{},
wpi::math::Rotation3d{},
{zero, zero, zero, zero},
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const frc::Pose3d& pose = odometry.GetPose();
const wpi::math::Pose3d& pose = odometry.GetPose();
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
@@ -48,12 +48,12 @@ TEST_F(SwerveDriveOdometry3dTest, Initialize) {
TEST_F(SwerveDriveOdometry3dTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
m_odometry.ResetPosition(wpi::math::Rotation3d{}, {zero, zero, zero, zero},
Pose3d{});
m_odometry.Update(frc::Rotation3d{}, {zero, zero, zero, zero});
m_odometry.Update(wpi::math::Rotation3d{}, {zero, zero, zero, zero});
auto pose = m_odometry.Update(frc::Rotation3d{},
auto pose = m_odometry.Update(wpi::math::Rotation3d{},
{position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
@@ -68,9 +68,9 @@ TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) {
SwerveModulePosition bl{18.85_m, -90_deg};
SwerveModulePosition br{42.15_m, -26.565_deg};
m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero},
m_odometry.ResetPosition(wpi::math::Rotation3d{}, {zero, zero, zero, zero},
Pose3d{});
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg},
auto pose = m_odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg},
{fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
@@ -80,12 +80,12 @@ TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometry3dTest, GyroAngleReset) {
m_odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg},
m_odometry.ResetPosition(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg},
{zero, zero, zero, zero}, Pose3d{});
SwerveModulePosition position{0.5_m, 0_deg};
auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg},
auto pose = m_odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg},
{position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
@@ -100,7 +100,7 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry3d<4> odometry{
kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
kinematics, wpi::math::Rotation3d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;
@@ -116,8 +116,8 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
@@ -140,8 +140,8 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
br.angle = moduleStates[3].angle;
auto xhat = odometry.Update(
frc::Rotation3d{groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad}},
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())
@@ -165,7 +165,7 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry3d<4> odometry{
kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}};
kinematics, wpi::math::Rotation3d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;
@@ -181,8 +181,8 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
@@ -205,7 +205,7 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
wpi::math::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation().ToTranslation2d())

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

@@ -13,7 +13,7 @@
#include "wpi/math/trajectory/TrajectoryConfig.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
using namespace frc;
using namespace wpi::math;
static constexpr double kEpsilon = 0.01;
@@ -78,7 +78,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry<4> odometry{
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
kinematics, wpi::math::Rotation2d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;
@@ -94,8 +94,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
@@ -119,7 +119,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -143,7 +143,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
SwerveDriveOdometry<4> odometry{
kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}};
kinematics, wpi::math::Rotation2d{}, {zero, zero, zero, zero}};
SwerveModulePosition fl;
SwerveModulePosition fr;
@@ -159,8 +159,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
units::second_t dt = 20_ms;
units::second_t t = 0_s;
wpi::units::second_t dt = 20_ms;
wpi::units::second_t t = 0_s;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
@@ -183,7 +183,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();

10
wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp
View File

@@ -8,16 +8,16 @@
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
TEST(SwerveModulePositionTest, Equality) {
frc::SwerveModulePosition position1{2_m, 90_deg};
frc::SwerveModulePosition position2{2_m, 90_deg};
wpi::math::SwerveModulePosition position1{2_m, 90_deg};
wpi::math::SwerveModulePosition position2{2_m, 90_deg};
EXPECT_EQ(position1, position2);
}
TEST(SwerveModulePositionTest, Inequality) {
frc::SwerveModulePosition position1{1_m, 90_deg};
frc::SwerveModulePosition position2{2_m, 90_deg};
frc::SwerveModulePosition position3{1_m, 89_deg};
wpi::math::SwerveModulePosition position1{1_m, 90_deg};
wpi::math::SwerveModulePosition position2{2_m, 90_deg};
wpi::math::SwerveModulePosition position3{1_m, 89_deg};
EXPECT_NE(position1, position2);
EXPECT_NE(position1, position3);

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

@@ -10,15 +10,15 @@
static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleStateTest, Optimize) {
frc::Rotation2d angleA{45_deg};
frc::SwerveModuleState refA{-2_mps, 180_deg};
wpi::math::Rotation2d angleA{45_deg};
wpi::math::SwerveModuleState refA{-2_mps, 180_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
frc::Rotation2d angleB{-50_deg};
frc::SwerveModuleState refB{4.7_mps, 41_deg};
wpi::math::Rotation2d angleB{-50_deg};
wpi::math::SwerveModuleState refB{4.7_mps, 41_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -4.7, kEpsilon);
@@ -26,15 +26,15 @@ TEST(SwerveModuleStateTest, Optimize) {
}
TEST(SwerveModuleStateTest, NoOptimize) {
frc::Rotation2d angleA{0_deg};
frc::SwerveModuleState refA{2_mps, 89_deg};
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 89_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
frc::Rotation2d angleB{0_deg};
frc::SwerveModuleState refB{-2_mps, -2_deg};
wpi::math::Rotation2d angleB{0_deg};
wpi::math::SwerveModuleState refB{-2_mps, -2_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -2.0, kEpsilon);
@@ -42,85 +42,85 @@ TEST(SwerveModuleStateTest, NoOptimize) {
}
TEST(SwerveModuleStateTest, CosineScaling) {
frc::Rotation2d angleA{0_deg};
frc::SwerveModuleState refA{2_mps, 45_deg};
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 45_deg};
refA.CosineScale(angleA);
EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleB{45_deg};
frc::SwerveModuleState refB{2_mps, 45_deg};
wpi::math::Rotation2d angleB{45_deg};
wpi::math::SwerveModuleState refB{2_mps, 45_deg};
refB.CosineScale(angleB);
EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleC{-45_deg};
frc::SwerveModuleState refC{2_mps, 45_deg};
wpi::math::Rotation2d angleC{-45_deg};
wpi::math::SwerveModuleState refC{2_mps, 45_deg};
refC.CosineScale(angleC);
EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleD{135_deg};
frc::SwerveModuleState refD{2_mps, 45_deg};
wpi::math::Rotation2d angleD{135_deg};
wpi::math::SwerveModuleState refD{2_mps, 45_deg};
refD.CosineScale(angleD);
EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleE{-135_deg};
frc::SwerveModuleState refE{2_mps, 45_deg};
wpi::math::Rotation2d angleE{-135_deg};
wpi::math::SwerveModuleState refE{2_mps, 45_deg};
refE.CosineScale(angleE);
EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleF{180_deg};
frc::SwerveModuleState refF{2_mps, 45_deg};
wpi::math::Rotation2d angleF{180_deg};
wpi::math::SwerveModuleState refF{2_mps, 45_deg};
refF.CosineScale(angleF);
EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleG{0_deg};
frc::SwerveModuleState refG{-2_mps, 45_deg};
wpi::math::Rotation2d angleG{0_deg};
wpi::math::SwerveModuleState refG{-2_mps, 45_deg};
refG.CosineScale(angleG);
EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleH{45_deg};
frc::SwerveModuleState refH{-2_mps, 45_deg};
wpi::math::Rotation2d angleH{45_deg};
wpi::math::SwerveModuleState refH{-2_mps, 45_deg};
refH.CosineScale(angleH);
EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleI{-45_deg};
frc::SwerveModuleState refI{-2_mps, 45_deg};
wpi::math::Rotation2d angleI{-45_deg};
wpi::math::SwerveModuleState refI{-2_mps, 45_deg};
refI.CosineScale(angleI);
EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleJ{135_deg};
frc::SwerveModuleState refJ{-2_mps, 45_deg};
wpi::math::Rotation2d angleJ{135_deg};
wpi::math::SwerveModuleState refJ{-2_mps, 45_deg};
refJ.CosineScale(angleJ);
EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleK{-135_deg};
frc::SwerveModuleState refK{-2_mps, 45_deg};
wpi::math::Rotation2d angleK{-135_deg};
wpi::math::SwerveModuleState refK{-2_mps, 45_deg};
refK.CosineScale(angleK);
EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleL{180_deg};
frc::SwerveModuleState refL{-2_mps, 45_deg};
wpi::math::Rotation2d angleL{180_deg};
wpi::math::SwerveModuleState refL{-2_mps, 45_deg};
refL.CosineScale(angleL);
EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
@@ -128,16 +128,16 @@ TEST(SwerveModuleStateTest, CosineScaling) {
}
TEST(SwerveModuleStateTest, Equality) {
frc::SwerveModuleState state1{2_mps, 90_deg};
frc::SwerveModuleState state2{2_mps, 90_deg};
wpi::math::SwerveModuleState state1{2_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
EXPECT_EQ(state1, state2);
}
TEST(SwerveModuleStateTest, Inequality) {
frc::SwerveModuleState state1{1_mps, 90_deg};
frc::SwerveModuleState state2{2_mps, 90_deg};
frc::SwerveModuleState state3{1_mps, 89_deg};
wpi::math::SwerveModuleState state1{1_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
wpi::math::SwerveModuleState state3{1_mps, 89_deg};
EXPECT_NE(state1, state2);
EXPECT_NE(state1, state3);

6
wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const ChassisSpeeds kExpectedData =
} // namespace
TEST(ChassisSpeedsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const DifferentialDriveKinematics kExpectedData =
} // namespace
TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const DifferentialDriveWheelSpeeds kExpectedData =
} // namespace
TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -17,8 +17,8 @@ const MecanumDriveKinematics kExpectedData = MecanumDriveKinematics{
} // namespace
TEST(MecanumDriveKinematicsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const MecanumDriveWheelPositions kExpectedData =
} // namespace
TEST(MecanumDriveWheelPositionsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const MecanumDriveWheelSpeeds kExpectedData =
} // namespace
TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.cpp
View File

@@ -8,14 +8,14 @@
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp"
using namespace frc;
using namespace wpi::math;
struct SwerveDriveKinematicsProtoTestData {
using Type = SwerveDriveKinematics<4>;
inline static const Type kTestData{
frc::Translation2d{1.0_m, 0.9_m}, frc::Translation2d{1.1_m, -0.8_m},
frc::Translation2d{-1.2_m, 0.7_m}, frc::Translation2d{-1.3_m, -0.6_m}};
wpi::math::Translation2d{1.0_m, 0.9_m}, wpi::math::Translation2d{1.1_m, -0.8_m},
wpi::math::Translation2d{-1.2_m, 0.7_m}, wpi::math::Translation2d{-1.3_m, -0.6_m}};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetModules(), data.GetModules());

6
wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const SwerveModulePosition kExpectedData =
} // namespace
TEST(SwerveModulePositionProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

6
wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
@@ -16,8 +16,8 @@ const SwerveModuleState kExpectedData =
} // namespace
TEST(SwerveModuleStateProtoTest, Roundtrip) {
wpi::ProtobufMessage<decltype(kExpectedData)> message;
wpi::SmallVector<uint8_t, 64> buf;
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
ASSERT_TRUE(message.Pack(buf, kExpectedData));
auto unpacked_data = message.Unpack(buf);

4
wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::ChassisSpeeds>;
using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
const ChassisSpeeds kExpectedData{
ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveKinematics>;
const DifferentialDriveKinematics kExpectedData{
DifferentialDriveKinematics{1.74_m}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::DifferentialDriveWheelPositions>;
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
const DifferentialDriveWheelPositions kExpectedData{
DifferentialDriveWheelPositions{1.74_m, 35.04_m}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
const DifferentialDriveWheelSpeeds kExpectedData{
DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
using StructType = wpi::util::Struct<wpi::math::MecanumDriveKinematics>;
const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{
Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m},
Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}};

4
wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelPositions>;
const MecanumDriveWheelPositions kExpectedData{
MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
const MecanumDriveWheelSpeeds kExpectedData{
MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
} // namespace

6
wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.cpp
View File

@@ -8,14 +8,14 @@
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/struct/SwerveDriveKinematicsStruct.hpp"
using namespace frc;
using namespace wpi::math;
struct SwerveDriveKinematicsStructTestData {
using Type = SwerveDriveKinematics<4>;
inline static const Type kTestData{
frc::Translation2d{1.0_m, 0.9_m}, frc::Translation2d{1.1_m, -0.8_m},
frc::Translation2d{-1.2_m, 0.7_m}, frc::Translation2d{-1.3_m, -0.6_m}};
wpi::math::Translation2d{1.0_m, 0.9_m}, wpi::math::Translation2d{1.1_m, -0.8_m},
wpi::math::Translation2d{-1.2_m, 0.7_m}, wpi::math::Translation2d{-1.3_m, -0.6_m}};
static void CheckEq(const Type& testData, const Type& data) {
EXPECT_EQ(testData.GetModules(), data.GetModules());

4
wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::SwerveModulePosition>;
using StructType = wpi::util::Struct<wpi::math::SwerveModulePosition>;
const SwerveModulePosition kExpectedData{
SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}};
} // namespace

4
wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp
View File

@@ -6,11 +6,11 @@
#include "wpi/math/kinematics/SwerveModuleState.hpp"
using namespace frc;
using namespace wpi::math;
namespace {
using StructType = wpi::Struct<frc::SwerveModuleState>;
using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
const SwerveModuleState kExpectedData{
SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}};
} // namespace