Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use immutable member functions in ChassisSpeeds (#7545)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-15 16:09:34 -08:00
committed by GitHub
parent 945d416d07
commit 03f0fc4dea
24 changed files with 163 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View File

@@ -23,7 +23,7 @@ class ChassisSpeedsTest {
final var duration = 1.0;
final var dt = 0.01;
final var speeds = ChassisSpeeds.discretize(target, duration);
final var speeds = target.discretize(duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
@@ -60,9 +60,8 @@ class ChassisSpeedsTest {
}
@Test
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);
void testToRobotRelative() {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
@@ -71,9 +70,9 @@ class ChassisSpeedsTest {
}
@Test
void testFromRobotRelativeSpeeds() {
void testToFieldRelative() {
final var chassisSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),

6
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -227,8 +227,7 @@ class MecanumDriveKinematicsTest {
@Test
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.desaturate(5.5);
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7).desaturate(5.5);
double factor = 5.5 / 7.0;
@@ -241,8 +240,7 @@ class MecanumDriveKinematicsTest {
@Test
void testDesaturateNegativeSpeeds() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7);
wheelSpeeds.desaturate(5.5);
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7).desaturate(5.5);
final double kFactor = 5.5 / 7.0;

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

@@ -15,7 +15,7 @@ TEST(ChassisSpeedsTest, Discretize) {
constexpr units::second_t duration = 1_s;
constexpr units::second_t dt = 10_ms;
const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
const auto speeds = target.Discretize(duration);
const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
frc::Pose2d pose;
@@ -29,18 +29,19 @@ TEST(ChassisSpeedsTest, Discretize) {
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
frc::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);

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

@@ -205,8 +205,8 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Desaturate(5.5_mps);
auto wheelSpeeds =
MecanumDriveWheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(5.5_mps);
double kFactor = 5.5 / 7.0;
@@ -217,8 +217,8 @@ TEST_F(MecanumDriveKinematicsTest, Desaturate) {
}
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps};
wheelSpeeds.Desaturate(5.5_mps);
auto wheelSpeeds =
MecanumDriveWheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(5.5_mps);
constexpr double kFactor = 5.5 / 7.0;