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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,7 +16,8 @@ TEST(ChassisSpeedsTest, Discretize) {
constexpr wpi::units::second_t dt = 10_ms;
const auto speeds = target.Discretize(duration);
const wpi::math::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};
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
@@ -41,7 +42,8 @@ TEST(ChassisSpeedsTest, ToRobotRelative) {
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
wpi::math::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);

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

@@ -16,7 +16,8 @@ using namespace wpi::math;
TEST(DifferentialDriveOdometry3dTest, Initialize) {
DifferentialDriveOdometry3d odometry{
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}}};
wpi::math::Pose3d{1_m, 2_m, 0_m,
wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}}};
const wpi::math::Pose3d& pose = odometry.GetPose();
@@ -27,11 +28,12 @@ TEST(DifferentialDriveOdometry3dTest, Initialize) {
}
TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
DifferentialDriveOdometry3d odometry{wpi::math::Rotation3d{0_deg, 0_deg, 45_deg},
0_m, 0_m};
DifferentialDriveOdometry3d odometry{
wpi::math::Rotation3d{0_deg, 0_deg, 45_deg}, 0_m, 0_m};
const auto& pose = odometry.Update(wpi::math::Rotation3d{0_deg, 0_deg, 135_deg},
0_m, wpi::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);

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

@@ -28,7 +28,8 @@ class MecanumDriveOdometry3dTest : public ::testing::Test {
TEST_F(MecanumDriveOdometry3dTest, Initialize) {
MecanumDriveOdometry3d odometry{
kinematics, wpi::math::Rotation3d{}, zero,
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::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 wpi::math::Pose3d& pose = odometry.GetPose();
@@ -82,7 +83,8 @@ TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
odometry.ResetPosition(wpi::math::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};
@@ -99,19 +101,22 @@ TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) {
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
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}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
wpi::math::MecanumDriveOdometry3d odometry{
kinematics, wpi::math::Rotation3d{}, wheelPositions};
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));
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);
@@ -140,8 +145,9 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::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())
@@ -162,19 +168,22 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
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}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveOdometry3d odometry{kinematics, wpi::math::Rotation3d{},
wheelPositions};
wpi::math::MecanumDriveOdometry3d odometry{
kinematics, wpi::math::Rotation3d{}, wheelPositions};
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));
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);

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

@@ -80,19 +80,22 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
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}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
wheelPositions};
wheelPositions};
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));
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);
@@ -120,10 +123,10 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
wheelPositions);
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
wheelPositions);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -143,19 +146,22 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
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}};
wpi::math::Translation2d{-1_m, -1_m},
wpi::math::Translation2d{-1_m, 1_m}};
wpi::math::MecanumDriveWheelPositions wheelPositions;
wpi::math::MecanumDriveOdometry odometry{kinematics, wpi::math::Rotation2d{},
wheelPositions};
wheelPositions};
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));
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);
@@ -185,7 +191,8 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = odometry.Update(
wpi::math::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();

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

@@ -7,8 +7,10 @@
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
TEST(MecanumDriveWheelSpeedsTest, Plus) {
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 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 wpi::math::MecanumDriveWheelSpeeds result = left + right;
@@ -19,8 +21,10 @@ TEST(MecanumDriveWheelSpeedsTest, Plus) {
}
TEST(MecanumDriveWheelSpeedsTest, Minus) {
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 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 wpi::math::MecanumDriveWheelSpeeds result = left - right;
@@ -31,7 +35,8 @@ TEST(MecanumDriveWheelSpeedsTest, Minus) {
}
TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
const wpi::math::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 wpi::math::MecanumDriveWheelSpeeds result = -speeds;
@@ -42,7 +47,8 @@ TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
}
TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
const wpi::math::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 wpi::math::MecanumDriveWheelSpeeds result = speeds * 2;
@@ -53,7 +59,8 @@ TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
}
TEST(MecanumDriveWheelSpeedsTest, Division) {
const wpi::math::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 wpi::math::MecanumDriveWheelSpeeds result = speeds / 2;

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

@@ -35,7 +35,8 @@ TEST_F(SwerveDriveOdometry3dTest, Initialize) {
m_kinematics,
wpi::math::Rotation3d{},
{zero, zero, zero, zero},
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::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 wpi::math::Pose3d& pose = odometry.GetPose();
@@ -140,8 +141,9 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
br.angle = moduleStates[3].angle;
auto xhat = odometry.Update(
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
wpi::math::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())

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

@@ -117,10 +117,10 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
{fl, fr, bl, br});
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -183,7 +183,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
wpi::math::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();

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

@@ -13,9 +13,10 @@ using namespace wpi::math;
struct SwerveDriveKinematicsProtoTestData {
using Type = SwerveDriveKinematics<4>;
inline static const Type kTestData{
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}};
inline static const Type kTestData{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());

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

@@ -10,7 +10,8 @@ using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
using StructType =
wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
const DifferentialDriveWheelPositions kExpectedData{
DifferentialDriveWheelPositions{1.74_m, 35.04_m}};
} // namespace

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

@@ -13,9 +13,10 @@ using namespace wpi::math;
struct SwerveDriveKinematicsStructTestData {
using Type = SwerveDriveKinematics<4>;
inline static const Type kTestData{
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}};
inline static const Type kTestData{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());