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

[wpimath] Add Translation2d/Translation3d slew rate limiter (#7806)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Adrien Bourdeaux
2025-02-25 22:06:00 -05:00
committed by GitHub
parent cd6fee7fea
commit 75321f1d84
4 changed files with 240 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -3,11 +3,17 @@
// the WPILib BSD license file in the root directory of this project.
#include <limits>
#include <numbers>
#include <gtest/gtest.h>
#include "frc/MathUtil.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}
TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
const frc::Translation2d translation1{0_m, 0_m};
const frc::Translation2d translation2{2_m, 2_m};
const frc::Translation2d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
const frc::Translation2d expected1{2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
const frc::Translation2d translation3{1_m, 1_m};
const frc::Translation2d translation4{3_m, 3_m};
const frc::Translation2d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
const frc::Translation2d expected2{
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
EXPECT_EQ(result2, expected2);
}
TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
const frc::Translation3d translation1{0_m, 0_m, 0_m};
const frc::Translation3d translation2{2_m, 2_m, 2_m};
const frc::Translation3d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
const frc::Translation3d expected1{2_m, 2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
const frc::Translation3d translation3{1_m, 1_m, 1_m};
const frc::Translation3d translation4{3_m, 3_m, 3_m};
const frc::Translation3d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
const frc::Translation3d expected2{
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
EXPECT_EQ(result2, expected2);
}