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

[wpimath] Rename ChassisSpeeds.fromDiscreteSpeeds() to discretize() (#5616)

Browse Source 
This better reflects what's actually going on mathematically.
This commit is contained in:
Tyler Veness
2023-09-08 20:14:59 -07:00
committed by GitHub
parent 8e05983a4a
commit a9ab08f48b
14 changed files with 101 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,20 +16,23 @@ class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
void testVeeringCorrection() {
final var duration = 1.0; // duration of observation
final var dt = 0.01; // time increment for simulation
void testDiscretize() {
final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
final var speeds = ChassisSpeeds.fromDiscreteSpeeds(target, duration);
final var duration = 1.0;
final var dt = 0.01;
final var speeds = ChassisSpeeds.discretize(target, duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
speeds.vyMetersPerSecond * dt,
speeds.omegaRadiansPerSecond * dt);
var pose = new Pose2d();
for (double time = 0; time < duration; time += dt) {
pose = pose.exp(twist);
}
final var result = pose; // For lambda capture
assertAll(
() -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
@@ -42,7 +45,7 @@ class ChassisSpeedsTest {
}
@Test
void testFieldRelativeConstruction() {
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));

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

@@ -8,7 +8,26 @@
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
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;
const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
const frc::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) {
pose = pose.Exp(twist);
}
EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
EXPECT_NEAR((target.omega * duration).value(),
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);