From 15dcdebe218f88109f4d75c393c05e321f6a4618 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Thu, 20 Feb 2025 20:12:48 -0800 Subject: [PATCH 01/34] [docs] Update contributing so breaking changes go to 2027 (#7809) --- CONTRIBUTING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 1ab9b13252..f5b54d4b04 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -80,7 +80,7 @@ xₖ₊₁ = Axₖ + Buₖ Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered. -During the build season, breaking changes or other changes intended for the next season can be created as a pull request against the development branch of WPILib. After the season is over, the changes in the development branch will be merged into main. +Particularly large and/or breaking changes should be targeted to the 2027 branch, which targets the [SystemCore Robot Controller](https://community.firstinspires.org/introducing-the-future-mobile-robot-controller). The intent is minimize changes for 2026, to allow development to focus on preparing for 2027. ### Merge Process From f9307de04c55e437eaf6b9f8320819ba8f7db9bf Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Thu, 20 Feb 2025 20:13:09 -0800 Subject: [PATCH 02/34] [docs] Document that /format is disabled (#7810) Add instructions for manual workarounds --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 3576047006..f7d65b0faa 100644 --- a/README.md +++ b/README.md @@ -136,6 +136,9 @@ If you have installed the FRC Toolchain to a directory other than the default, o Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes. +> [!NOTE] +> The `/format` action has been temporarily disabled. The individual formatting commands can be run locally as shown below. Alternately, the Lint and Format action for a PR will upload a patch file that can be downloaded and applied manually. + #### wpiformat wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms. From 0f0e93722ef2f3ee509b9f7b4d91c43449ab75aa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 25 Feb 2025 19:01:41 -0800 Subject: [PATCH 03/34] [ci] Upgrade to Clang 17 sanitizers (#7819) --- .github/workflows/sanitizers.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index c32a3a05b8..b929190135 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -29,11 +29,11 @@ jobs: ctest-env: "" ctest-flags: "" name: "${{ matrix.name }}" - runs-on: ubuntu-22.04 - container: wpilib/roborio-cross-ubuntu:2025-22.04 + runs-on: ubuntu-24.04 + container: wpilib/roborio-cross-ubuntu:2025-24.04 steps: - name: Install Dependencies - run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java clang-14 libprotobuf-dev protobuf-compiler ninja-build + run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-17 libprotobuf-dev protobuf-compiler ninja-build - name: Install sccache uses: mozilla-actions/sccache-action@v0.0.5 @@ -41,7 +41,7 @@ jobs: - uses: actions/checkout@v4 - name: configure - run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} .. + run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-17 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-17 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} .. env: SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }} SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }} From b0e588fd49f446b12e7d44229bca312d488ad2e0 Mon Sep 17 00:00:00 2001 From: Rain Heuer <63077980+rmheuer@users.noreply.github.com> Date: Tue, 25 Feb 2025 21:04:16 -0600 Subject: [PATCH 04/34] [glass] Update Field2D default field to 2025 (#7820) --- glass/src/lib/native/cpp/other/Field2D.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index 609a75b182..c4f61b3c40 100644 --- a/glass/src/lib/native/cpp/other/Field2D.cpp +++ b/glass/src/lib/native/cpp/other/Field2D.cpp @@ -224,8 +224,8 @@ class ObjectInfo { class FieldInfo { public: - static constexpr auto kDefaultWidth = 16.541052_m; - static constexpr auto kDefaultHeight = 8.211_m; + static constexpr auto kDefaultWidth = 17.5483_m; + static constexpr auto kDefaultHeight = 8.0519_m; explicit FieldInfo(Storage& storage); @@ -345,7 +345,7 @@ static bool InputPose(frc::Pose2d* pose) { } FieldInfo::FieldInfo(Storage& storage) - : m_builtin{storage.GetString("builtin", "2024 Crescendo")}, + : m_builtin{storage.GetString("builtin", "2025 Reefscape")}, m_filename{storage.GetString("image")}, m_width{storage.GetFloat("width", kDefaultWidth.to())}, m_height{storage.GetFloat("height", kDefaultHeight.to())}, From 517344fe8030c46bd87467832cdcbb9f51cfda92 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 25 Feb 2025 19:04:31 -0800 Subject: [PATCH 05/34] [wpimath] Fix another infinite loop in ArmFeedforward (#7823) --- .../native/cpp/controller/ArmFeedforward.cpp | 12 +- .../math/controller/ArmFeedforwardTest.java | 119 ++++++++++++++---- .../cpp/controller/ArmFeedforwardTest.cpp | 100 ++++++++++++--- 3 files changed, 186 insertions(+), 45 deletions(-) diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index df1f89d984..fbfdde2227 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate( sleipnir::Hessian hessianF{cost, xAD}; Eigen::SparseMatrix H = hessianF.Value(); - double error = std::numeric_limits::infinity(); - while (error > 1e-8) { + double error_k = std::numeric_limits::infinity(); + double error_k1 = std::abs(g.coeff(0)); + + // Loop until error stops decreasing or max iterations is reached + for (size_t iteration = 0; + iteration < 50 && error_k1 < (1.0 - 1e-10) * error_k; ++iteration) { + error_k = error_k1; + // Iterate via Newton's method. // // xₖ₊₁ = xₖ − H⁻¹g @@ -97,7 +103,7 @@ units::volt_t ArmFeedforward::Calculate( g = gradientF.Value(); H = hessianF.Value(); - error = std::abs(g.coeff(0)); + error_k1 = std::abs(g.coeff(0)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index 3c8748d35f..b18b81641e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java @@ -18,15 +18,13 @@ import java.util.function.BiFunction; import org.junit.jupiter.api.Test; class ArmFeedforwardTest { - private static final double ks = 0.5; - private static final double kg = 1; - private static final double kv = 1.5; - private static final double ka = 2; - private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka); - /** * Simulates a single-jointed arm and returns the final state. * + * @param ks The static gain, in volts. + * @param kv The velocity gain, in volt seconds per radian. + * @param ka The acceleration gain, in volt seconds² per radian. + * @param kg The gravity gain, in volts. * @param currentAngle The starting angle in radians. * @param currentVelocity The starting angular velocity in radians per second. * @param input The input voltage. @@ -34,7 +32,14 @@ class ArmFeedforwardTest { * @return The final state as a 2-vector of angle and angular velocity. */ private Matrix simulate( - double currentAngle, double currentVelocity, double input, double dt) { + double ks, + double kv, + double ka, + double kg, + double currentAngle, + double currentVelocity, + double input, + double dt) { final Matrix A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka}); final Matrix B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka}); @@ -61,47 +66,115 @@ class ArmFeedforwardTest { * Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt; * then simulates an arm using that voltage to verify correctness. * + * @param armFF The feedforward object. + * @param ks The static gain, in volts. + * @param kv The velocity gain, in volt seconds per radian. + * @param ka The acceleration gain, in volt seconds² per radian. + * @param kg The gravity gain, in volts. * @param currentAngle The starting angle in radians. * @param currentVelocity The starting angular velocity in radians per second. * @param input The input voltage. * @param dt The simulation time in seconds. */ private void calculateAndSimulate( - double currentAngle, double currentVelocity, double nextVelocity, double dt) { - final double input = - m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity); - assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12); + ArmFeedforward armFF, + double ks, + double kv, + double ka, + double kg, + double currentAngle, + double currentVelocity, + double nextVelocity, + double dt) { + final double input = armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity); + assertEquals( + nextVelocity, + simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0), + 1e-4); } @Test void testCalculate() { + final double ks = 0.5; + final double kv = 1.5; + final double ka = 2; + final double kg = 1; + final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka); + // calculate(angle, angular velocity) - assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002); - assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002); + assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002); + assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002); // calculate(currentAngle, currentVelocity, nextAngle, dt) - calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020); - calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020); - calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020); - calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020); + calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020); + calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020); + calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020); + calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020); + } + + @Test + void testCalculateIllConditionedModel() { + final double ks = 0.39671; + final double kv = 2.7167; + final double ka = 1e-2; + final double kg = 0.2708; + final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka); + + final double currentAngle = 1.0; + final double currentVelocity = 0.02; + final double nextVelocity = 0.0; + + var averageAccel = (nextVelocity - currentVelocity) / 0.02; + + assertEquals( + armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity), + ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle)); + } + + @Test + void testCalculateIllConditionedGradient() { + final double ks = 0.39671; + final double kv = 2.7167; + final double ka = 0.50799; + final double kg = 0.2708; + final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka); + + calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02); } @Test void testAchievableVelocity() { - assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002); - assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002); + final double ks = 0.5; + final double kv = 1.5; + final double ka = 2; + final double kg = 1; + final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka); + + assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002); + assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002); } @Test void testAchievableAcceleration() { - assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002); - assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002); - assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002); - assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002); + final double ks = 0.5; + final double kv = 1.5; + final double ka = 2; + final double kg = 1; + final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka); + + assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002); + assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002); + assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002); + assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002); } @Test void testNegativeGains() { + final double ks = 0.5; + final double kv = 1.5; + final double ka = 2; + final double kg = 1; + assertAll( () -> assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)), diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp index 66060c2020..157b10da75 100644 --- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp @@ -15,27 +15,32 @@ #include "units/time.h" #include "units/voltage.h" -static constexpr auto Ks = 0.5_V; -static constexpr auto Kv = 1.5_V / 1_rad_per_s; -static constexpr auto Ka = 2_V / 1_rad_per_s_sq; -static constexpr auto Kg = 1_V; - namespace { +using Ks_unit = decltype(1_V); +using Kv_unit = decltype(1_V / 1_rad_per_s); +using Ka_unit = decltype(1_V / 1_rad_per_s_sq); +using Kg_unit = decltype(1_V); + /** * Simulates a single-jointed arm and returns the final state. * + * @param Ks The static gain, in volts. + * @param Kv The velocity gain, in volt seconds per radian. + * @param Ka The acceleration gain, in volt seconds² per radian. + * @param Kg The gravity gain, in volts. * @param currentAngle The starting angle. * @param currentVelocity The starting angular velocity. * @param input The input voltage. * @param dt The simulation time. * @return The final state as a 2-vector of angle and angular velocity. */ -frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle, +frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg, + units::radian_t currentAngle, units::radians_per_second_t currentVelocity, units::volt_t input, units::second_t dt) { - constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}}; - constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}}; + frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}}; + frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}}; return frc::RK4( [&](const frc::Matrixd<2, 1>& x, @@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle, * Simulates a single-jointed arm and returns the final state. * * @param armFF The feedforward object. + * @param Ks The static gain, in volts. + * @param Kv The velocity gain, in volt seconds per radian. + * @param Ka The acceleration gain, in volt seconds² per radian. + * @param Kg The gravity gain, in volts. * @param currentAngle The starting angle. * @param currentVelocity The starting angular velocity. * @param input The input voltage. * @param dt The simulation time. */ -void CalculateAndSimulate(const frc::ArmFeedforward& armFF, +void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks, + Kv_unit Kv, Ka_unit Ka, Kg_unit Kg, units::radian_t currentAngle, units::radians_per_second_t currentVelocity, units::radians_per_second_t nextVelocity, units::second_t dt) { auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity); - EXPECT_NEAR(nextVelocity.value(), - Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12); + EXPECT_NEAR( + nextVelocity.value(), + Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1), + 1e-4); } } // namespace TEST(ArmFeedforwardTest, Calculate) { + constexpr auto Ks = 0.5_V; + constexpr auto Kv = 1.5_V / 1_rad_per_s; + constexpr auto Ka = 2_V / 1_rad_per_s_sq; + constexpr auto Kg = 1_V; frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; // Calculate(angle, angular velocity) @@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) { 0.002); // Calculate(currentAngle, currentVelocity, nextAngle, dt) - CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s, - 1.05_rad_per_s, 20_ms); - CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s, - 0.95_rad_per_s, 20_ms); - CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s, - 1.05_rad_per_s, 20_ms); - CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s, - 0.95_rad_per_s, 20_ms); + CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s, 1.05_rad_per_s, 20_ms); + CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad, + 1_rad_per_s, 0.95_rad_per_s, 20_ms); + CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad, + 1_rad_per_s, 1.05_rad_per_s, 20_ms); + CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad, + 1_rad_per_s, 0.95_rad_per_s, 20_ms); +} + +TEST(ArmFeedforwardTest, CalculateIllConditionedModel) { + constexpr auto Ks = 0.39671_V; + constexpr auto Kv = 2.7167_V / 1_rad_per_s; + constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq; + constexpr auto Kg = 0.2708_V; + frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; + + constexpr auto currentAngle = 1_rad; + constexpr auto currentVelocity = 0.02_rad_per_s; + constexpr auto nextVelocity = 0_rad_per_s; + + constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms; + + EXPECT_DOUBLE_EQ( + armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(), + (Ks + Kv * currentVelocity + Ka * averageAccel + + Kg * units::math::cos(currentAngle)) + .value()); +} + +TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) { + constexpr auto Ks = 0.39671_V; + constexpr auto Kv = 2.7167_V / 1_rad_per_s; + constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq; + constexpr auto Kg = 0.2708_V; + frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; + + CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s, + 0_rad_per_s, 20_ms); } TEST(ArmFeedforwardTest, AchievableVelocity) { + constexpr auto Ks = 0.5_V; + constexpr auto Kv = 1.5_V / 1_rad_per_s; + constexpr auto Ka = 2_V / 1_rad_per_s_sq; + constexpr auto Kg = 1_V; frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; + EXPECT_NEAR(armFF .MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad, 1_rad_per_s_sq) @@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) { } TEST(ArmFeedforwardTest, AchievableAcceleration) { + constexpr auto Ks = 0.5_V; + constexpr auto Kv = 1.5_V / 1_rad_per_s; + constexpr auto Ka = 2_V / 1_rad_per_s_sq; + constexpr auto Kg = 1_V; frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka}; + EXPECT_NEAR(armFF .MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad, 1_rad_per_s) @@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) { } TEST(ArmFeedforwardTest, NegativeGains) { + constexpr auto Ks = 0.5_V; + constexpr auto Kv = 1.5_V / 1_rad_per_s; + constexpr auto Ka = 2_V / 1_rad_per_s_sq; + constexpr auto Kg = 1_V; frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka}; + EXPECT_EQ(armFF.GetKv().value(), 0); EXPECT_EQ(armFF.GetKa().value(), 0); } From cd6fee7fea30e3bcdabfa6851939a295b5c3e5ac Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 25 Feb 2025 19:05:05 -0800 Subject: [PATCH 06/34] [sysid] Refactor feedback analysis (#7827) --- .../native/cpp/analysis/FeedbackAnalysis.cpp | 59 ++++++++----------- .../cpp/analysis/FeedbackAnalysisTest.cpp | 30 +++++++++- 2 files changed, 53 insertions(+), 36 deletions(-) diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 7821812bd4..bd21ab7a1a 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -18,6 +19,7 @@ using namespace sysid; using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); +using Matrix1d = Eigen::Matrix; FeedbackGains sysid::CalculatePositionFeedbackGains( const FeedbackControllerPreset& preset, const LQRParameters& params, @@ -26,39 +28,32 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( return {0.0, 0.0}; } - // If acceleration requires no effort, velocity becomes an input for position - // control. We choose an appropriate model in this case to avoid numerical + // If acceleration for position control requires no effort, velocity becomes + // an input. We choose an appropriate model in this case to avoid numerical // instabilities in the LQR. - if (Ka > 1E-7) { - // Create a position system from our feedforward gains. - frc::LinearSystem<2, 1, 1> system{ - frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}}, - frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0}, - frc::Matrixd<1, 1>{0.0}}; - // Create an LQR with 2 states to control -- position and velocity. - frc::LinearQuadraticRegulator<2, 1> controller{ - system, {params.qp, params.qv}, {params.r}, preset.period}; - // Compensate for any latency from sensor measurements, filtering, etc. + if (std::abs(Ka) < 1e-7) { + // System has position state and velocity input + frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0}, + Matrix1d{1.0}, Matrix1d{0.0}}; + + frc::LinearQuadraticRegulator<1, 1> controller{ + system, {params.qp}, {params.r}, preset.period}; controller.LatencyCompensate(system, preset.period, preset.measurementDelay); - return { - controller.K(0, 0) * preset.outputConversionFactor, - controller.K(0, 1) * preset.outputConversionFactor / - (preset.normalized ? 1 : units::second_t{preset.period}.value())}; + return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0}; } - // This is our special model to avoid instabilities in the LQR. - auto system = frc::LinearSystem<1, 1, 1>( - Eigen::Matrix{0.0}, Eigen::Matrix{1.0}, - Eigen::Matrix{1.0}, Eigen::Matrix{0.0}); - // Create an LQR with one state -- position. - frc::LinearQuadraticRegulator<1, 1> controller{ - system, {params.qp}, {params.r}, preset.period}; - // Compensate for any latency from sensor measurements, filtering, etc. + auto system = frc::LinearSystemId::IdentifyPositionSystem( + Kv_t{Kv}, Ka_t{Ka}); + + frc::LinearQuadraticRegulator<2, 1> controller{ + system, {params.qp, params.qv}, {params.r}, preset.period}; controller.LatencyCompensate(system, preset.period, preset.measurementDelay); - return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0}; + return {controller.K(0, 0) * preset.outputConversionFactor, + controller.K(0, 1) * preset.outputConversionFactor / + (preset.normalized ? 1 : units::second_t{preset.period}.value())}; } FeedbackGains sysid::CalculateVelocityFeedbackGains( @@ -69,20 +64,16 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains( } // If acceleration for velocity control requires no effort, the feedback - // control gains approach zero. We special-case it here because numerical - // instabilities arise in LQR otherwise. - if (Ka < 1E-7) { + // control gains approach zero. We special-case it here to avoid numerical + // instabilities in LQR. + if (std::abs(Ka) < 1E-7) { return {0.0, 0.0}; } - // Create a velocity system from our feedforward gains. - frc::LinearSystem<1, 1, 1> system{ - frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka}, - frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}}; - // Create an LQR controller with 1 state -- velocity. + auto system = frc::LinearSystemId::IdentifyVelocitySystem( + Kv_t{Kv}, Ka_t{Ka}); frc::LinearQuadraticRegulator<1, 1> controller{ system, {params.qv}, {params.r}, preset.period}; - // Compensate for any latency from sensor measurements, filtering, etc. controller.LatencyCompensate(system, preset.period, preset.measurementDelay); return {controller.K(0, 0) * preset.outputConversionFactor / diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp index 92b26ca763..5ff5a501e4 100644 --- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -7,7 +7,7 @@ #include "sysid/analysis/FeedbackAnalysis.h" #include "sysid/analysis/FeedbackControllerPreset.h" -TEST(FeedbackAnalysisTest, Velocity1) { +TEST(FeedbackAnalysisTest, VelocitySystem1) { auto Kv = 3.060; auto Ka = 0.327; @@ -20,7 +20,7 @@ TEST(FeedbackAnalysisTest, Velocity1) { EXPECT_NEAR(Kd, 0.00, 0.05); } -TEST(FeedbackAnalysisTest, Velocity2) { +TEST(FeedbackAnalysisTest, VelocitySystem2) { auto Kv = 0.0693; auto Ka = 0.1170; @@ -33,6 +33,19 @@ TEST(FeedbackAnalysisTest, Velocity2) { EXPECT_NEAR(Kd, 0.00, 0.05); } +TEST(FeedbackAnalysisTest, VelocitySystemWithSmallKa) { + auto Kv = 3.060; + auto Ka = 0.0; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka); + + EXPECT_NEAR(Kp, 0.00, 0.05); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + TEST(FeedbackAnalysisTest, VelocityConversion) { auto Kv = 0.0693; auto Ka = 0.1170; @@ -117,6 +130,19 @@ TEST(FeedbackAnalysisTest, Position) { EXPECT_NEAR(Kd, 2.48, 0.05); } +TEST(FeedbackAnalysisTest, PositionWithSmallKa) { + auto Kv = 3.060; + auto Ka = 1e-10; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka); + + EXPECT_NEAR(Kp, 19.97, 0.05); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) { auto Kv = 3.060; auto Ka = 0.327; From 75321f1d843a2a4f07db2b0213208ca64d811645 Mon Sep 17 00:00:00 2001 From: Adrien Bourdeaux Date: Tue, 25 Feb 2025 22:06:00 -0500 Subject: [PATCH 07/34] [wpimath] Add Translation2d/Translation3d slew rate limiter (#7806) Co-authored-by: Tyler Veness Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../java/edu/wpi/first/math/MathUtil.java | 61 +++++++++++++++++ .../src/main/native/include/frc/MathUtil.h | 67 +++++++++++++++++++ .../java/edu/wpi/first/math/MathUtilTest.java | 53 +++++++++++++++ wpimath/src/test/native/cpp/MathUtilTest.cpp | 59 ++++++++++++++++ 4 files changed, 240 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 5bf89d7e81..1836005cee 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -4,6 +4,9 @@ package edu.wpi.first.math; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + /** Math utility functions. */ public final class MathUtil { private MathUtil() { @@ -209,4 +212,62 @@ public final class MathUtil { double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound); return Math.abs(error) < tolerance; } + + /** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation2d limited to maxVelocity + */ + public static Translation2d slewRateLimit( + Translation2d current, Translation2d next, double dt, double maxVelocity) { + if (maxVelocity < 0) { + Exception e = new IllegalArgumentException(); + MathSharedStore.reportError( + "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); + return next; + } + Translation2d diff = next.minus(current); + double dist = diff.getNorm(); + if (dist < 1e-9) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current.plus(diff.times(maxVelocity * dt / dist)); + } + return next; + } + + /** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation3d limited to maxVelocity + */ + public static Translation3d slewRateLimit( + Translation3d current, Translation3d next, double dt, double maxVelocity) { + if (maxVelocity < 0) { + Exception e = new IllegalArgumentException(); + MathSharedStore.reportError( + "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace()); + return next; + } + Translation3d diff = next.minus(current); + double dist = diff.getNorm(); + if (dist < 1e-9) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current.plus(diff.times(maxVelocity * dt / dist)); + } + return next; + } } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index a0447ee2a1..e6ec32908d 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -10,9 +10,15 @@ #include #include +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Translation3d.h" #include "units/angle.h" #include "units/base.h" +#include "units/length.h" #include "units/math.h" +#include "units/time.h" +#include "units/velocity.h" +#include "wpimath/MathShared.h" namespace frc { @@ -207,4 +213,65 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x, std::signed_integral auto y) { return x - FloorDiv(x, y) * y; } + +/** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation2d limited to maxVelocity + */ +constexpr Translation2d SlewRateLimit(const Translation2d& current, + const Translation2d& next, + units::second_t dt, + units::meters_per_second_t maxVelocity) { + if (maxVelocity < 0_mps) { + wpi::math::MathSharedStore::ReportError( + "maxVelocity must be a non-negative number, got {}!", maxVelocity); + return next; + } + Translation2d diff = next - current; + units::meter_t dist = diff.Norm(); + if (dist < 1e-9_m) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current + diff * (maxVelocity * dt / dist); + } + return next; +} + +/** + * Limits translation velocity. + * + * @param current Translation at current timestep. + * @param next Translation at next timestep. + * @param dt Timestep duration. + * @param maxVelocity Maximum translation velocity. + * @return Returns the next Translation3d limited to maxVelocity + */ +constexpr Translation3d SlewRateLimit(const Translation3d& current, + const Translation3d& next, + units::second_t dt, + units::meters_per_second_t maxVelocity) { + if (maxVelocity < 0_mps) { + wpi::math::MathSharedStore::ReportError( + "maxVelocity must be a non-negative number, got {}!", maxVelocity); + return next; + } + Translation3d diff = next - current; + units::meter_t dist = diff.Norm(); + if (dist < 1e-9_m) { + return next; + } + if (dist > maxVelocity * dt) { + // Move maximum allowed amount in direction of the difference + return current + diff * (maxVelocity * dt / dist); + } + return next; +} + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index 44f7c50a0e..93ef1ef734 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.UtilityClassTest; import org.junit.jupiter.api.Test; @@ -167,4 +169,55 @@ class MathUtilTest extends UtilityClassTest { assertFalse(MathUtil.isNear(400, -315, 5, 0, 360)); assertFalse(MathUtil.isNear(400, 395, 5, 0, 360)); } + + @Test + void testSlewRateTranslation2dUnchanged() { + Translation2d translation1 = new Translation2d(0, 0); + Translation2d translation2 = new Translation2d(2, 2); + + Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50); + + Translation2d expected1 = new Translation2d(2, 2); + + assertEquals(expected1, result1); + } + + @Test + void testSlewRateTranslation2dChanged() { + Translation2d translation3 = new Translation2d(1, 1); + Translation2d translation4 = new Translation2d(3, 3); + + Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2); + + Translation2d expected2 = + new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0)); + + assertEquals(expected2, result2); + } + + @Test + void testSlewRateTranslation3dUnchanged() { + Translation3d translation1 = new Translation3d(0, 0, 0); + Translation3d translation2 = new Translation3d(2, 2, 2); + + Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50); + + Translation3d expected1 = new Translation3d(2, 2, 2); + + assertEquals(expected1, result1); + } + + @Test + void testSlewRateTranslation3dChanged() { + Translation3d translation3 = new Translation3d(1, 1, 1); + Translation3d translation4 = new Translation3d(3, 3, 3); + + Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2); + + Translation3d expected2 = + new Translation3d( + 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0)); + + assertEquals(expected2, result2); + } } diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index cee983185b..fc9405ce38 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -3,11 +3,17 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #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); +} From 822457d45bd6bde1ba4abf8655d6eaa2dcdd6ec9 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 25 Feb 2025 19:07:51 -0800 Subject: [PATCH 08/34] =?UTF-8?q?[wpimath]=20Fix=20feedforward=20returning?= =?UTF-8?q?=20NaN=20when=20k=E1=B5=A5=20=3D=200=20(#7790)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- wpimath/algorithms.md | 34 +++++++++++++++++-- .../math/controller/ElevatorFeedforward.java | 4 +-- .../controller/SimpleMotorFeedforward.java | 4 +-- .../frc/controller/ElevatorFeedforward.h | 4 +-- .../frc/controller/SimpleMotorFeedforward.h | 4 +-- 5 files changed, 40 insertions(+), 10 deletions(-) diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index a413894887..094c7c8ac5 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -92,7 +92,7 @@ Substitute these into the feedforward equation. uₖ = kₛ sgn(x) + kᵥxₖ₊₁ ``` -Simplify the model when ka ≠ 0 +Simplify the model when kₐ ≠ 0 ``` uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) @@ -107,6 +107,21 @@ where B_d = A⁻¹(eᴬᵀ - I)B ``` +When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method. + +``` + [A B] + [0 0]T [A_d B_d] +e = [ 0 I ] + + [0 B] + [0 0]T [1 BT] +e = [0 1] + +A_d = 1 +B_d = BT +``` + ## Elevator feedforward ### Derivation @@ -199,7 +214,7 @@ Substitute these into the feedforward equation. uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁ ``` -Simplify the model when ka ≠ 0 +Simplify the model when kₐ ≠ 0 ``` uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) @@ -214,6 +229,21 @@ where B_d = A⁻¹(eᴬᵀ - I)B ``` +When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method. + +``` + [A B] + [0 0]T [A_d B_d] +e = [ 0 I ] + + [0 B] + [0 0]T [1 BT] +e = [0 1] + +A_d = 1 +B_d = BT +``` + ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I ### Derivation diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index e33de90b15..245fcf8443 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -202,13 +202,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ */ public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Elevator_feedforward for derivation - if (ka == 0.0) { + if (ka < 1e-9) { return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity; } else { double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); - double B_d = 1.0 / A * (A_d - 1.0) * B; + double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B; return kg + ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 7c04f4e4ec..37907fc342 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -186,13 +186,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria */ public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation - if (ka == 0.0) { + if (ka < 1e-9) { return ks * Math.signum(nextVelocity) + kv * nextVelocity; } else { double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); - double B_d = 1.0 / A * (A_d - 1.0) * B; + double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B; return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); } } diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 93575276c5..1c974a9b0a 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -133,13 +133,13 @@ class ElevatorFeedforward { units::unit_t currentVelocity, units::unit_t nextVelocity) const { // See wpimath/algorithms.md#Elevator_feedforward for derivation - if (kA == decltype(kA)(0)) { + if (kA < decltype(kA)(1e-9)) { return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity; } else { double A = -kV.value() / kA.value(); double B = 1.0 / kA.value(); double A_d = gcem::exp(A * m_dt.value()); - double B_d = 1.0 / A * (A_d - 1.0) * B; + double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B; return kG + kS * wpi::sgn(currentVelocity) + units::volt_t{ 1.0 / B_d * diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index eb48be1c31..4e675096f4 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -109,13 +109,13 @@ class SimpleMotorFeedforward { units::unit_t currentVelocity, units::unit_t nextVelocity) const { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation - if (kA == decltype(kA)(0)) { + if (kA < decltype(kA)(1e-9)) { return kS * wpi::sgn(nextVelocity) + kV * nextVelocity; } else { double A = -kV.value() / kA.value(); double B = 1.0 / kA.value(); double A_d = gcem::exp(A * m_dt.value()); - double B_d = 1.0 / A * (A_d - 1.0) * B; + double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B; return kS * wpi::sgn(currentVelocity) + units::volt_t{ 1.0 / B_d * From a7ae22d7640c3395e9a8ca37dc8c5fe0b7a2a559 Mon Sep 17 00:00:00 2001 From: arbessette <115647688+arbessette@users.noreply.github.com> Date: Thu, 27 Feb 2025 14:02:13 -0500 Subject: [PATCH 09/34] [docs] Update code of conduct (#7833) --- CODE_OF_CONDUCT.md | 65 ++++++++++++++++------------------------------ 1 file changed, 23 insertions(+), 42 deletions(-) diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md index 3d3b7e8627..a14b32a0a1 100644 --- a/CODE_OF_CONDUCT.md +++ b/CODE_OF_CONDUCT.md @@ -1,69 +1,47 @@ -# Contributor Covenant Code of Conduct +# Contributor Community Code of Conduct ## Our Pledge -We as members, contributors, and leaders pledge to make participation in our -community a harassment-free experience for everyone, regardless of age, body -size, visible or invisible disability, ethnicity, sex characteristics, gender -identity and expression, level of experience, education, socio-economic status, -nationality, personal appearance, race, religion, or sexual identity -and orientation. +As members, contributors, and leaders, we commit to fostering a community where everyone feels safe, respected, and valued. We are dedicated to ensuring that participation in this community is harassment-free, inclusive, and welcoming, regardless of age, body type, abilities (visible or invisible), ethnicity, gender identity or expression, sexual orientation, socioeconomic background, education, nationality, personal appearance, race, or religion. + +Above all, we pledge to act with integrity, kindness, and empathy—striving to be not only good participants but also good humans. -We pledge to act and interact in ways that contribute to an open, welcoming, -diverse, inclusive, and healthy community. ## Our Standards -Examples of behavior that contributes to a positive environment for our -community include: +Positive and respectful behavior is essential to creating a thriving community. This includes: -* Exhibiting Gracious Professionalism® at all times. Gracious Professionalism +* Practicing **Gracious Professionalism®** at all times. Gracious Professionalism is a way of doing things that encourages high-quality work, emphasizes the value of others, and respects individuals and the community. -* Demonstrating empathy and kindness toward other people -* Being respectful of differing opinions, viewpoints, and experiences -* Giving and gracefully accepting constructive feedback -* Accepting responsibility and apologizing to those affected by our mistakes, - and learning from the experience -* Focusing on what is best not just for us as individuals, but for the - overall community +* Showing empathy, kindness, and patience. +* Respecting diverse perspectives and experiences. +* Giving and receiving constructive feedback with openness and humility. +* Owning mistakes, apologizing when necessary, and learning from them. +* Prioritizing the well-being and success of the entire community over individual interests. -Examples of unacceptable behavior include: +Unacceptable behavior include: -* The use of sexualized language or imagery, and sexual attention or - advances of any kind +* Using sexualized language, imagery, or making inappropriate advances * Trolling, insulting or derogatory comments, and personal or political attacks -* Public or private harassment -* Publishing others' private information, such as a physical or email - address, without their explicit permission -* Other conduct which could reasonably be considered inappropriate in a - professional setting +* Harassment in any form, whether public or private. +* Sharing private information (e.g., email or physical addresses) without explicit consent. + +* Any behavior that is unprofessional, harmful, or exclusionary. ## Enforcement Responsibilities -Community leaders are responsible for clarifying and enforcing our standards of -acceptable behavior and will take appropriate and fair corrective action in -response to any behavior that they deem inappropriate, threatening, offensive, -or harmful. - -Community leaders have the right and responsibility to remove, edit, or reject -comments, commits, code, wiki edits, issues, and other contributions that are -not aligned to this Code of Conduct, and will communicate reasons for moderation -decisions when appropriate. +Community leaders are responsible for maintaining these standards and will take appropriate action to address any behavior deemed harmful, threatening, or inappropriate. Actions may include removing content, issuing warnings, or, when necessary, banning individuals. Moderation decisions will be communicated transparently where appropriate. ## Scope -This Code of Conduct applies within all community spaces, and also applies when -an individual is officially representing the community in public spaces. -Examples of representing our community include using an official e-mail address, -posting via an official social media account, or acting as an appointed -representative at an online or offline event. +This Code of Conduct applies to all community spaces, events, and instances where individuals represent the community (e.g., official email accounts, social media posts, or in-person/virtual events). ## Enforcement Instances of abusive, harassing, or otherwise unacceptable behavior may be reported to the community leaders responsible for enforcement at -[conduct@wpilib.org](mailto:conduct@wpilib.org). +[wpilib@wpi.edu](mailto:wpilib@wpi.edu). All complaints will be reviewed and investigated promptly and fairly. All community leaders are obligated to respect the privacy and security of the @@ -115,6 +93,9 @@ individual, or aggression toward or disparagement of classes of individuals. **Consequence**: A permanent ban from any sort of public interaction within the community. +## A Note on Kindness +Building a community isn’t just about rules—it’s about connection. Every interaction is an opportunity to be understanding, compassionate, and supportive. Being a good human is the key to our ethos. + ## Attribution This Code of Conduct is adapted from the [Contributor Covenant][homepage], From 93bf6c70ba8de62cc6b994619524778c2f65e309 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 2 Mar 2025 10:46:50 -0800 Subject: [PATCH 10/34] [build] Remove sources from ceres-cpp (#7844) --- shared/ceres.gradle | 1 - 1 file changed, 1 deletion(-) diff --git a/shared/ceres.gradle b/shared/ceres.gradle index 550fd6c9e7..beaaa113f3 100644 --- a/shared/ceres.gradle +++ b/shared/ceres.gradle @@ -4,7 +4,6 @@ nativeUtils { groupId = "edu.wpi.first.thirdparty.frc2024.ceres" artifactId = "ceres-cpp" headerClassifier = "headers" - sourceClassifier = "sources" ext = "zip" version = '2.2-3' targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms) From 0ad595c33c52828394e64ec2a080eb6c2eacce80 Mon Sep 17 00:00:00 2001 From: crueter Date: Mon, 3 Mar 2025 19:21:38 -0500 Subject: [PATCH 11/34] [wpimath] Add Debouncer type and time setters (#7839) Signed-off-by: swurl Co-authored-by: Tyler Veness --- .../edu/wpi/first/math/filter/Debouncer.java | 42 ++++++++++++++++++- .../native/include/frc/filter/Debouncer.h | 32 ++++++++++++++ .../wpi/first/math/filter/DebouncerTest.java | 18 ++++++++ .../test/native/cpp/filter/DebouncerTest.cpp | 17 ++++++++ 4 files changed, 107 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java index 9a281e83f7..a19de59f0b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java @@ -21,8 +21,8 @@ public class Debouncer { kBoth } - private final double m_debounceTimeSeconds; - private final DebounceType m_debounceType; + private double m_debounceTimeSeconds; + private DebounceType m_debounceType; private boolean m_baseline; private double m_prevTimeSeconds; @@ -86,4 +86,42 @@ public class Debouncer { return m_baseline; } } + + /** + * Sets the time to debounce. + * + * @param time The number of seconds the value must change from baseline for the filtered value to + * change. + */ + public void setDebounceTime(double time) { + m_debounceTimeSeconds = time; + } + + /** + * Gets the time to debounce. + * + * @return The number of seconds the value must change from baseline for the filtered value to + * change. + */ + public double getDebounceTime() { + return m_debounceTimeSeconds; + } + + /** + * Sets the debounce type. + * + * @param type Which type of state change the debouncing will be performed on. + */ + public void setDebounceType(DebounceType type) { + m_debounceType = type; + } + + /** + * Gets the debounce type. + * + * @return Which type of state change the debouncing will be performed on. + */ + public DebounceType getDebounceType() { + return m_debounceType; + } } diff --git a/wpimath/src/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h index 9ab3e0e168..45ed5df8e6 100644 --- a/wpimath/src/main/native/include/frc/filter/Debouncer.h +++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h @@ -48,6 +48,38 @@ class WPILIB_DLLEXPORT Debouncer { */ bool Calculate(bool input); + /** + * Sets the time to debounce. + * + * @param time The number of seconds the value must change from baseline + * for the filtered value to change. + */ + constexpr void SetDebounceTime(units::second_t time) { + m_debounceTime = time; + } + + /** + * Gets the time to debounce. + * + * @return The number of seconds the value must change from baseline + * for the filtered value to change. + */ + constexpr units::second_t GetDebounceTime() const { return m_debounceTime; } + + /** + * Set the debounce type. + * + * @param type Which type of state change the debouncing will be performed on. + */ + constexpr void SetDebounceType(DebounceType type) { m_debounceType = type; } + + /** + * Get the debounce type. + * + * @return Which type of state change the debouncing will be performed on. + */ + constexpr DebounceType GetDebounceType() const { return m_debounceType; } + private: units::second_t m_debounceTime; bool m_baseline; diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java index b6334de009..8cf8fd8812 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java @@ -4,7 +4,9 @@ package edu.wpi.first.math.filter; +import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertSame; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.util.WPIUtilJNI; @@ -67,4 +69,20 @@ class DebouncerTest { assertFalse(debouncer.calculate(false)); } + + @Test + void debounceParamsTest() { + var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth); + + assertEquals(debouncer.getDebounceTime(), 0.02); + assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kBoth); + + debouncer.setDebounceTime(0.1); + + assertEquals(debouncer.getDebounceTime(), 0.1); + + debouncer.setDebounceType(Debouncer.DebounceType.kFalling); + + assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling); + } } diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp index d232739b7a..e1a990964b 100644 --- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp +++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp @@ -56,3 +56,20 @@ TEST_F(DebouncerTest, DebounceBoth) { EXPECT_FALSE(debouncer.Calculate(false)); } + +TEST_F(DebouncerTest, DebounceParams) { + frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth}; + + EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms); + EXPECT_TRUE(debouncer.GetDebounceType() == + frc::Debouncer::DebounceType::kBoth); + + debouncer.SetDebounceTime(100_ms); + + EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms); + + debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling); + + EXPECT_TRUE(debouncer.GetDebounceType() == + frc::Debouncer::DebounceType::kFalling); +} From 1a835fec01fa167936f4ac6bb969fdbf038584c7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 4 Mar 2025 18:52:17 -0800 Subject: [PATCH 12/34] [wpimath] Fix singularities in Ellipse2d::Nearest() (#7851) The problem was ill-conditioned if either semiaxis had zero length. --- wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 17ef087c19..9c4fe4fc1e 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -34,11 +34,12 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const { slp::pow(y - rotPoint.Y().value(), 2)); // (x − x_c)²/a² + (y − y_c)²/b² = 1 - problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) / - (m_xSemiAxis.value() * m_xSemiAxis.value()) + - slp::pow(y - m_center.Y().value(), 2) / - (m_ySemiAxis.value() * m_ySemiAxis.value()) == - 1); + // b²(x − x_c)² + a²(y − y_c)² = a²b² + double a2 = m_xSemiAxis.value() * m_xSemiAxis.value(); + double b2 = m_ySemiAxis.value() * m_ySemiAxis.value(); + problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) + + a2 * slp::pow(y - m_center.Y().value(), 2) == + a2 * b2); problem.Solve(); From 71b6e8ec583ab23c5de014ff899ec956f04096fd Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 6 Mar 2025 23:15:22 -0800 Subject: [PATCH 13/34] [wpiutil] Avoid including execinfo.h for Emscripten target (#7854) --- wpiutil/src/main/native/unix/StackTrace.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wpiutil/src/main/native/unix/StackTrace.cpp b/wpiutil/src/main/native/unix/StackTrace.cpp index cc753c26b9..ba7156e709 100644 --- a/wpiutil/src/main/native/unix/StackTrace.cpp +++ b/wpiutil/src/main/native/unix/StackTrace.cpp @@ -4,7 +4,9 @@ #include "wpi/StackTrace.h" +#ifndef __EMSCRIPTEN__ #include +#endif #include @@ -16,7 +18,7 @@ namespace wpi { std::string GetStackTraceDefault(int offset) { -#ifndef __ANDROID__ +#if !defined(__ANDROID__) && !defined(__EMSCRIPTEN__) void* stackTrace[128]; int stackSize = backtrace(stackTrace, 128); char** mangledSymbols = backtrace_symbols(stackTrace, stackSize); @@ -40,7 +42,7 @@ std::string GetStackTraceDefault(int offset) { return std::string{trace.str()}; #else - // backtrace_symbols not supported on android + // backtrace_symbols not supported return ""; #endif } From 1efaaefd78a634599b522fc2420c7283722554fb Mon Sep 17 00:00:00 2001 From: Jack Cammarata <50638415+PascalSkylake@users.noreply.github.com> Date: Fri, 14 Mar 2025 13:05:15 -0400 Subject: [PATCH 14/34] [wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850) Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular. This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed. To fix the bug, a couple things needed to be changed: all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor, In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular, P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively. Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements. Co-authored-by: Tyler Veness --- .../estimator/MerweScaledSigmaPoints.java | 6 +- .../math/estimator/UnscentedKalmanFilter.java | 117 +++++++++--- .../frc/estimator/MerweScaledSigmaPoints.h | 4 +- .../frc/estimator/UnscentedKalmanFilter.h | 78 ++++++-- .../frc/estimator/UnscentedTransform.h | 38 +++- .../estimator/UnscentedKalmanFilterTest.java | 173 +++++++++++++++--- .../estimator/UnscentedKalmanFilterTest.cpp | 166 ++++++++++++++--- 7 files changed, 494 insertions(+), 88 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java index 7c378dff47..d3131a83e9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java @@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints { } /** - * Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) - * of the filter. + * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root + * covariance (s) of the filter. * * @param x An array of the means. * @param s Square-root covariance of the filter. @@ -86,6 +86,8 @@ public class MerweScaledSigmaPoints { // 2 * states + 1 by states Matrix sigmas = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1)); + + // equation (17) sigmas.setColumn(0, x); for (int k = 0; k < m_states.getNum(); k++) { var xPlusU = x.plus(U.extractColumnVector(k)); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index c64ec85cf9..1bc474b4c1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -35,9 +35,9 @@ import org.ejml.simple.SimpleMatrix; * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf * chapter 9 "Stochastic control theory". * - *

This class implements a square-root-form unscented Kalman filter (SR-UKF). For more - * information about the SR-UKF, see https://www.researchgate.net/publication/3908304. + *

This class implements a square-root-form unscented Kalman filter (SR-UKF). The main reason for + * this is to guarantee that the covariance matrix remains positive definite. For more information + * about the SR-UKF, see https://www.researchgate.net/publication/3908304. * * @param Number of states. * @param Number of inputs. @@ -105,7 +105,7 @@ public class UnscentedKalmanFilter x = meanFunc.apply(sigmas, Wm); + // Form an intermediate matrix S⁻ as: + // + // [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}] + // + // the part of equations (20) and (24) within the "qr{}" Matrix Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum())); for (int i = 0; i < 2 * s.getNum(); i++) { Sbar.setColumn( @@ -214,8 +223,24 @@ public class UnscentedKalmanFilter newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true))); - newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false); + // Compute the square-root covariance of the sigma points + // + // We transpose S⁻ first because we formed it by horizontally + // concatenating each part; it should be vertical so we can take + // the QR decomposition as defined in the "QR Decomposition" passage + // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION" + // + // The resulting matrix R is the square-root covariance S, but it + // is upper triangular, so we need to transpose it. + // + // equations (20) and (24) + Matrix newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose()); + + // Update or downdate the square-root covariance with (𝒳₀-x̂) + // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative. + // + // equations (21) and (25) + newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true); return new Pair<>(x, newS); } @@ -256,7 +281,7 @@ public class UnscentedKalmanFilter getP() { - return m_S.transpose().times(m_S); + return m_S.times(m_S.transpose()); } /** @@ -280,7 +305,7 @@ public class UnscentedKalmanFilter newP) { - m_S = newP.lltDecompose(false); + m_S = newP.lltDecompose(true); } /** @@ -347,14 +372,28 @@ public class UnscentedKalmanFilter x = sigmas.extractColumnVector(i); m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds)); } + // Pass the predicted sigmas (𝒳) through the Unscented Transform + // to compute the prior state mean and covariance + // + // equations (18) (19) and (20) var ret = squareRootUnscentedTransform( m_states, @@ -459,7 +498,15 @@ public class UnscentedKalmanFilter sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1)); var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S); for (int i = 0; i < m_pts.getNumSigmas(); i++) { @@ -467,7 +514,11 @@ public class UnscentedKalmanFilter Pxy = new Matrix<>(m_states, rows); for (int i = 0; i < m_pts.getNumSigmas(); i++) { - // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i] var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat); var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose(); Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i))); } - // K = (P_{xy} / S_yᵀ) / S_y - // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y - // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ + // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This + // is equivalent to MATLAB's \ operator, so we need to rearrange to use + // that. + // + // K = (P_{xy} / S_{y}ᵀ) / S_{y} + // K = (S_{y} \ P_{xy})ᵀ / S_{y} + // K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ + // + // equation (27) Matrix K = Sy.transpose() .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose())) .transpose(); - // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ) + // Compute the posterior state mean + // + // x̂ = x̂⁻ + K(y − ŷ⁻) + // + // second part of equation (27) m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat))); + // Compute the intermediate matrix U for downdating + // the square-root covariance + // + // equation (28) Matrix U = K.times(Sy); + + // Downdate the posterior square-root state covariance + // + // equation (29) for (int i = 0; i < rows.getNum(); i++) { - m_S.rankUpdate(U.extractColumnVector(i), -1, false); + m_S.rankUpdate(U.extractColumnVector(i), -1, true); } } } diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h index ffc65eee85..2605d5e03e 100644 --- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h +++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h @@ -51,7 +51,7 @@ class MerweScaledSigmaPoints { /** * Computes the sigma points for an unscented Kalman filter given the mean - * (x) and square-root covariance(S) of the filter. + * (x) and square-root covariance (S) of the filter. * * @param x An array of the means. * @param S Square-root covariance of the filter. @@ -68,6 +68,8 @@ class MerweScaledSigmaPoints { Matrixd U = eta * S; Matrixd sigmas; + + // equation (17) sigmas.template block(0, 0) = x; for (int k = 0; k < States; ++k) { sigmas.template block(0, k + 1) = diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 87ee5ffd29..21838870a8 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -43,7 +43,9 @@ namespace frc { * "Stochastic control theory". * *

This class implements a square-root-form unscented Kalman filter - * (SR-UKF). For more information about the SR-UKF, see + * (SR-UKF). The main reason for this is to guarantee that the covariance + * matrix remains positive definite. + * For more information about the SR-UKF, see * https://www.researchgate.net/publication/3908304. * * @tparam States Number of states. @@ -108,7 +110,7 @@ class UnscentedKalmanFilter { } /** - * Constructs an unscented Kalman filter with custom mean, residual, and + * Constructs an Unscented Kalman filter with custom mean, residual, and * addition functions. Using custom functions for arithmetic can be useful if * you have angles in the state or measurements, because they allow you to * correctly account for the modular nature of angle arithmetic. @@ -189,7 +191,7 @@ class UnscentedKalmanFilter { /** * Returns the reconstructed error covariance matrix P. */ - StateMatrix P() const { return m_S.transpose() * m_S; } + StateMatrix P() const { return m_S * m_S.transpose(); } /** * Set the current square-root error covariance matrix S by taking the square @@ -197,7 +199,7 @@ class UnscentedKalmanFilter { * * @param P The error covariance matrix P. */ - void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); } + void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); } /** * Returns the state estimate x-hat. @@ -252,14 +254,28 @@ class UnscentedKalmanFilter { DiscretizeAQ(contA, m_contQ, m_dt, &discA, &discQ); Eigen::internal::llt_inplace::blocked(discQ); + // Generate sigma points around the state mean + // + // equation (17) Matrixd sigmas = m_pts.SquareRootSigmaPoints(m_xHat, m_S); + // Project each sigma point forward in time according to the + // dynamics f(x, u) + // + // sigmas = 𝒳ₖ₋₁ + // sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability + // + // equation (18) for (int i = 0; i < m_pts.NumSigmas(); ++i) { StateVector x = sigmas.template block(0, i); m_sigmasF.template block(0, i) = RK4(m_f, x, u, dt); } + // Pass the predicted sigmas (𝒳) through the Unscented Transform + // to compute the prior state mean and covariance + // + // equations (18) (19) and (20) auto [xHat, S] = SquareRootUnscentedTransform( m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX, discQ.template triangularView()); @@ -367,7 +383,15 @@ class UnscentedKalmanFilter { Matrixd discR = DiscretizeR(R, m_dt); Eigen::internal::llt_inplace::blocked(discR); - // Transform sigma points into measurement space + // Generate new sigma points from the prior mean and covariance + // and transform them into measurement space using h(x, u) + // + // sigmas = 𝒳 + // sigmasH = 𝒴 + // + // This differs from equation (22) which uses + // the prior sigma points, regenerating them allows + // multiple measurement updates per time update Matrixd sigmasH; Matrixd sigmas = m_pts.SquareRootSigmaPoints(m_xHat, m_S); @@ -376,16 +400,26 @@ class UnscentedKalmanFilter { h(sigmas.template block(0, i), u); } - // Mean and covariance of prediction passed through UT + // Pass the predicted measurement sigmas through the Unscented Transform + // to compute the mean predicted measurement and square-root innovation + // covariance. + // + // equations (23) (24) and (25) auto [yHat, Sy] = SquareRootUnscentedTransform( sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY, discR.template triangularView()); - // Compute cross covariance of the state and the measurements + // Compute cross covariance of the predicted state and measurement sigma + // points given as: + // + // 2n + // P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ + // i=0 + // + // equation (26) Matrixd Pxy; Pxy.setZero(); for (int i = 0; i < m_pts.NumSigmas(); ++i) { - // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i] Pxy += m_pts.Wc(i) * (residualFuncX(m_sigmasF.template block(0, i), m_xHat)) * @@ -393,21 +427,39 @@ class UnscentedKalmanFilter { .transpose(); } - // K = (P_{xy} / S_yᵀ) / S_y - // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y - // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ + // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This + // is equivalent to MATLAB's \ operator, so we need to rearrange to use + // that. + // + // K = (P_{xy} / S_{y}ᵀ) / S_{y} + // K = (S_{y} \ P_{xy})ᵀ / S_{y} + // K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ + // + // equation (27) Matrixd K = Sy.transpose() .fullPivHouseholderQr() .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose())) .transpose(); - // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ) + // Compute the posterior state mean + // + // x̂ = x̂⁻ + K(y − ŷ⁻) + // + // second part of equation (27) m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat)); + // Compute the intermediate matrix U for downdating + // the square-root covariance + // + // equation (28) Matrixd U = K * Sy; + + // Downdate the posterior square-root state covariance + // + // equation (29) for (int i = 0; i < Rows; i++) { - Eigen::internal::llt_inplace::rankUpdate( + Eigen::internal::llt_inplace::rankUpdate( m_S, U.template block(0, i), -1); } } diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h index 4a99e90157..0002638d79 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -47,12 +47,21 @@ SquareRootUnscentedTransform( const Vectord&)> residualFunc, const Matrixd& squareRootR) { - // New mean is usually just the sum of the sigmas * weight: - // n - // dot = Σ W[k] Xᵢ[k] - // k=1 + // New mean is usually just the sum of the sigmas * weights: + // + // 2n + // x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ + // i=0 + // + // equations (19) and (23) in the paper show this, + // but we allow a custom function, usually for angle wrapping Vectord x = meanFunc(sigmas, Wm); + // Form an intermediate matrix S⁻ as: + // + // [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}] + // + // the part of equations (20) and (24) within the "qr{}" Matrixd Sbar; for (int i = 0; i < States * 2; i++) { Sbar.template block(0, i) = @@ -61,14 +70,29 @@ SquareRootUnscentedTransform( } Sbar.template block(0, States * 2) = squareRootR; - // Merwe defines the QR decomposition as Aᵀ = QR + // Compute the square-root covariance of the sigma points. + // + // We transpose S⁻ first because we formed it by horizontally + // concatenating each part; it should be vertical so we can take + // the QR decomposition as defined in the "QR Decomposition" passage + // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION" + // + // The resulting matrix R is the square-root covariance S, but it + // is upper triangular, so we need to transpose it. + // + // equations (20) and (24) Matrixd S = Sbar.transpose() .householderQr() .matrixQR() .template block(0, 0) - .template triangularView(); + .template triangularView() + .transpose(); - Eigen::internal::llt_inplace::rankUpdate( + // Update or downdate the square-root covariance with (𝒳₀-x̂) + // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative. + // + // equations (21) and (25) + Eigen::internal::llt_inplace::rankUpdate( S, residualFunc(sigmas.template block(0, 0), x), Wc[0]); return std::make_tuple(x, S); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index c5a6cd11a5..c5252a412d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -18,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.math.numbers.N5; import edu.wpi.first.math.system.Discretization; import edu.wpi.first.math.system.NumericalIntegration; @@ -26,11 +28,13 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.ArrayList; +import java.util.Collections; import java.util.List; import org.junit.jupiter.api.Test; class UnscentedKalmanFilterTest { - private static Matrix getDynamics(Matrix x, Matrix u) { + private static Matrix driveDynamics(Matrix x, Matrix u) { var motors = DCMotor.getCIM(2); // var gLow = 15.32; // Low gear ratio @@ -63,17 +67,17 @@ class UnscentedKalmanFilterTest { } @SuppressWarnings("PMD.UnusedFormalParameter") - private static Matrix getLocalMeasurementModel(Matrix x, Matrix u) { + private static Matrix driveLocalMeasurementModel(Matrix x, Matrix u) { return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0)); } @SuppressWarnings("PMD.UnusedFormalParameter") - private static Matrix getGlobalMeasurementModel(Matrix x, Matrix u) { + private static Matrix driveGlobalMeasurementModel(Matrix x, Matrix u) { return x.copy(); } @Test - void testInit() { + void testDriveInit() { var dtSeconds = 0.005; assertDoesNotThrow( () -> { @@ -81,8 +85,8 @@ class UnscentedKalmanFilterTest { new UnscentedKalmanFilter<>( Nat.N5(), Nat.N3(), - UnscentedKalmanFilterTest::getDynamics, - UnscentedKalmanFilterTest::getLocalMeasurementModel, + UnscentedKalmanFilterTest::driveDynamics, + UnscentedKalmanFilterTest::driveLocalMeasurementModel, VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), VecBuilder.fill(0.0001, 0.01, 0.01), AngleStatistics.angleMean(2), @@ -95,10 +99,10 @@ class UnscentedKalmanFilterTest { var u = VecBuilder.fill(12.0, 12.0); observer.predict(u, dtSeconds); - var localY = getLocalMeasurementModel(observer.getXhat(), u); + var localY = driveLocalMeasurementModel(observer.getXhat(), u); observer.correct(u, localY); - var globalY = getGlobalMeasurementModel(observer.getXhat(), u); + var globalY = driveGlobalMeasurementModel(observer.getXhat(), u); var R = StateSpaceUtil.makeCovarianceMatrix( Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01)); @@ -106,7 +110,7 @@ class UnscentedKalmanFilterTest { Nat.N5(), u, globalY, - UnscentedKalmanFilterTest::getGlobalMeasurementModel, + UnscentedKalmanFilterTest::driveGlobalMeasurementModel, R, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -116,16 +120,16 @@ class UnscentedKalmanFilterTest { } @Test - void testConvergence() { - double dtSeconds = 0.005; - double rbMeters = 0.8382 / 2.0; // Robot radius + void testDriveConvergence() { + final double dtSeconds = 0.005; + final double rbMeters = 0.8382 / 2.0; // Robot radius UnscentedKalmanFilter observer = new UnscentedKalmanFilter<>( Nat.N5(), Nat.N3(), - UnscentedKalmanFilterTest::getDynamics, - UnscentedKalmanFilterTest::getLocalMeasurementModel, + UnscentedKalmanFilterTest::driveDynamics, + UnscentedKalmanFilterTest::driveLocalMeasurementModel, VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), VecBuilder.fill(0.0001, 0.5, 0.5), AngleStatistics.angleMean(2), @@ -149,7 +153,7 @@ class UnscentedKalmanFilterTest { NumericalJacobian.numericalJacobianU( Nat.N5(), Nat.N2(), - UnscentedKalmanFilterTest::getDynamics, + UnscentedKalmanFilterTest::driveDynamics, new Matrix<>(Nat.N5(), Nat.N1()), new Matrix<>(Nat.N2(), Nat.N1())); @@ -164,7 +168,7 @@ class UnscentedKalmanFilterTest { var trueXhat = observer.getXhat(); double totalTime = trajectory.getTotalTimeSeconds(); - for (int i = 0; i < (totalTime / dtSeconds); i++) { + for (int i = 0; i < (totalTime / dtSeconds); ++i) { var ref = trajectory.sample(dtSeconds * i); double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)); double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)); @@ -177,25 +181,27 @@ class UnscentedKalmanFilterTest { vl, vr); - Matrix localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1())); + Matrix localY = + driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1())); var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5); observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev))); var rdot = nextR.minus(r).div(dtSeconds); - u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); + u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); observer.predict(u, dtSeconds); r = nextR; trueXhat = - NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds); + NumericalIntegration.rk4( + UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dtSeconds); } - var localY = getLocalMeasurementModel(trueXhat, u); + var localY = driveLocalMeasurementModel(trueXhat, u); observer.correct(u, localY); - var globalY = getGlobalMeasurementModel(trueXhat, u); + var globalY = driveGlobalMeasurementModel(trueXhat, u); var R = StateSpaceUtil.makeCovarianceMatrix( Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5)); @@ -203,7 +209,7 @@ class UnscentedKalmanFilterTest { Nat.N5(), u, globalY, - UnscentedKalmanFilterTest::getGlobalMeasurementModel, + UnscentedKalmanFilterTest::driveGlobalMeasurementModel, R, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -241,7 +247,7 @@ class UnscentedKalmanFilterTest { Matrix ref = VecBuilder.fill(100); Matrix u = VecBuilder.fill(0); - for (int i = 0; i < (2.0 / dt); i++) { + for (int i = 0; i < (2.0 / dt); ++i) { observer.predict(u, dt); u = discB.solve(ref.minus(discA.times(ref))); @@ -269,4 +275,125 @@ class UnscentedKalmanFilterTest { assertTrue(observer.getP().isEqual(P, 1e-9)); } + + // Second system, single motor feedforward estimator + private static Matrix motorDynamics(Matrix x, Matrix u) { + double v = x.get(1, 0); + double kV = x.get(2, 0); + double kA = x.get(3, 0); + + double V = u.get(0, 0); + + double a = -kV / kA * v + 1.0 / kA * V; + return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0); + } + + private static Matrix motorMeasurementModel(Matrix x, Matrix u) { + double p = x.get(0, 0); + double v = x.get(1, 0); + double kV = x.get(2, 0); + double kA = x.get(3, 0); + double V = u.get(0, 0); + + double a = -kV / kA * v + 1.0 / kA * V; + return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a); + } + + private static Matrix motorControlInput(double t) { + return MatBuilder.fill( + Nat.N1(), + Nat.N1(), + MathUtil.clamp( + 8 * Math.sin(Math.PI * Math.sqrt(2.0) * t) + + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t) + + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t), + -12.0, + 12.0)); + } + + @Test + void testMotorConvergence() { + final double dtSeconds = 0.01; + final int steps = 500; + final double true_kV = 3; + final double true_kA = 0.2; + + final double pos_stddev = 0.02; + final double vel_stddev = 0.1; + final double accel_stddev = 0.1; + + var states = + new ArrayList<>( + Collections.nCopies( + steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0))); + var inputs = + new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0))); + var measurements = + new ArrayList<>( + Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0))); + states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA)); + + var A = + MatBuilder.fill( + Nat.N4(), + Nat.N4(), + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + -true_kV / true_kA, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0); + var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0); + + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + for (int i = 0; i < steps; ++i) { + inputs.set(i, motorControlInput(i * dtSeconds)); + states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i)))); + measurements.set( + i, + motorMeasurementModel(states.get(i + 1), inputs.get(i)) + .plus( + StateSpaceUtil.makeWhiteNoiseVector( + VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev)))); + } + + var P0 = + MatBuilder.fill( + Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0, + 0.0, 0.0, 10); + + var observer = + new UnscentedKalmanFilter( + Nat.N4(), + Nat.N3(), + UnscentedKalmanFilterTest::motorDynamics, + UnscentedKalmanFilterTest::motorMeasurementModel, + VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10), + VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev), + dtSeconds); + + observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0)); + observer.setP(P0); + + for (int i = 0; i < steps; ++i) { + observer.predict(inputs.get(i), dtSeconds); + observer.correct(inputs.get(i), measurements.get(i)); + } + + assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5); + assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5); + } } diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index c2c6f824d8..21d2df98f4 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -2,7 +2,9 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include #include +#include #include #include @@ -12,15 +14,19 @@ #include "frc/StateSpaceUtil.h" #include "frc/estimator/AngleStatistics.h" #include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/system/Discretization.h" #include "frc/system/NumericalIntegration.h" #include "frc/system/NumericalJacobian.h" #include "frc/system/plant/DCMotor.h" +#include "frc/system/plant/LinearSystemId.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "units/moment_of_inertia.h" namespace { -frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { +// First test system, differential drive +frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x, + const frc::Vectord<2>& u) { auto motors = frc::DCMotor::CIM(2); // constexpr double Glow = 15.32; // Low gear ratio @@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { k1.value() * ((C1 * vr).value() + (C2 * Vr).value())}; } -frc::Vectord<3> LocalMeasurementModel( +frc::Vectord<3> DriveLocalMeasurementModel( const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) { return frc::Vectord<3>{x(2), x(3), x(4)}; } -frc::Vectord<5> GlobalMeasurementModel( +frc::Vectord<5> DriveGlobalMeasurementModel( const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) { return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)}; } -} // namespace -TEST(UnscentedKalmanFilterTest, Init) { +TEST(UnscentedKalmanFilterTest, DriveInit) { constexpr auto dt = 5_ms; - frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics, - LocalMeasurementModel, + frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics, + DriveLocalMeasurementModel, {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.01, 0.01}, frc::AngleMean<5, 5>(2), @@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) { frc::Vectord<2> u{12.0, 12.0}; observer.Predict(u, dt); - auto localY = LocalMeasurementModel(observer.Xhat(), u); + auto localY = DriveLocalMeasurementModel(observer.Xhat(), u); observer.Correct(u, localY); - auto globalY = GlobalMeasurementModel(observer.Xhat(), u); + auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01); - observer.Correct<5>(u, globalY, GlobalMeasurementModel, R, + observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R, frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); } -TEST(UnscentedKalmanFilterTest, Convergence) { +TEST(UnscentedKalmanFilterTest, DriveConvergence) { constexpr auto dt = 5_ms; constexpr auto rb = 0.8382_m / 2.0; // Robot radius - frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics, - LocalMeasurementModel, + frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics, + DriveLocalMeasurementModel, {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.5, 0.5}, frc::AngleMean<5, 5>(2), @@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) { frc::Vectord<5> r = frc::Vectord<5>::Zero(); frc::Vectord<2> u = frc::Vectord<2>::Zero(); - auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(), - frc::Vectord<2>::Zero()); + auto B = frc::NumericalJacobianU<5, 5, 2>( + DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero()); observer.SetXhat(frc::Vectord<5>{ trajectory.InitialPose().Translation().X().value(), @@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) { ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(), ref.pose.Rotation().Radians().value(), vl.value(), vr.value()}; - auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero()); + auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero()); observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); frc::Vectord<5> rdot = (nextR - r) / dt.value(); - u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero())); + u = B.householderQr().solve(rdot - + DriveDynamics(r, frc::Vectord<2>::Zero())); observer.Predict(u, dt); r = nextR; - trueXhat = frc::RK4(Dynamics, trueXhat, u, dt); + trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt); } - auto localY = LocalMeasurementModel(trueXhat, u); + auto localY = DriveLocalMeasurementModel(trueXhat, u); observer.Correct(u, localY); - auto globalY = GlobalMeasurementModel(trueXhat, u); + auto globalY = DriveGlobalMeasurementModel(trueXhat, u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); - observer.Correct<5>(u, globalY, GlobalMeasurementModel, R, + observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R, frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2) @@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) { EXPECT_NEAR(0.0, observer.Xhat(4), 0.1); } +TEST(UnscentedKalmanFilterTest, LinearUKF) { + constexpr units::second_t dt = 20_ms; + auto plant = frc::LinearSystemId::IdentifyVelocitySystem( + 0.02_V / 1_mps, 0.006_V / 1_mps_sq); + frc::UnscentedKalmanFilter<1, 1, 1> observer{ + [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return plant.A() * x + plant.B() * u; + }, + [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { + return plant.CalculateY(x, u); + }, + {0.05}, + {1.0}, + dt}; + + frc::Matrixd<1, 1> discA; + frc::Matrixd<1, 1> discB; + frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB); + + frc::Vectord<1> ref{100.0}; + frc::Vectord<1> u{0.0}; + + for (int i = 0; i < 2.0 / dt.value(); ++i) { + observer.Predict(u, dt); + + u = discB.householderQr().solve(ref - discA * ref); + } + + EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5); +} + TEST(UnscentedKalmanFilterTest, RoundTripP) { constexpr auto dt = 5_ms; @@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) { ASSERT_TRUE(observer.P().isApprox(P)); } + +// Second system, single motor feedforward estimator +frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x, + const frc::Vectord<1>& u) { + double v = x(1); + double kV = x(2); + double kA = x(3); + + double V = u(0); + + double a = -kV / kA * v + 1.0 / kA * V; + return frc::Vectord<4>{v, a, 0.0, 0.0}; +} + +frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x, + const frc::Vectord<1>& u) { + double p = x(0); + double v = x(1); + double kV = x(2); + double kA = x(3); + + double V = u(0); + + double a = -kV / kA * v + 1.0 / kA * V; + return frc::Vectord<3>{p, v, a}; +} + +frc::Vectord<1> MotorControlInput(double t) { + return frc::Vectord<1>{ + std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) + + 6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) + + 4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t), + -12.0, 12.0)}; +} + +TEST(UnscentedKalmanFilterTest, MotorConvergence) { + constexpr units::second_t dt = 10_ms; + constexpr int steps = 500; + constexpr double true_kV = 3; + constexpr double true_kA = 0.2; + + constexpr double pos_stddev = 0.02; + constexpr double vel_stddev = 0.1; + constexpr double accel_stddev = 0.1; + + std::vector> states(steps + 1); + std::vector> inputs(steps); + std::vector> measurements(steps); + states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}}; + + constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0}, + {0.0, -true_kV / true_kA, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0}}; + constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}}; + + frc::Matrixd<4, 4> discA; + frc::Matrixd<4, 1> discB; + frc::DiscretizeAB(A, B, dt, &discA, &discB); + + for (int i = 0; i < steps; ++i) { + inputs[i] = MotorControlInput(i * dt.value()); + states[i + 1] = discA * states[i] + discB * inputs[i]; + measurements[i] = + MotorMeasurementModel(states[i + 1], inputs[i]) + + frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev); + } + + frc::Vectord<4> P0{0.001, 0.001, 10, 10}; + + frc::UnscentedKalmanFilter<4, 1, 3> observer{ + MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10}, + wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt}; + + observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0}); + observer.SetP(P0.asDiagonal()); + + for (int i = 0; i < steps; ++i) { + observer.Predict(inputs[i], dt); + observer.Correct(inputs[i], measurements[i]); + } + + EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5); + EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5); +} + +} // namespace From dcd397e00715ed1b689e56ef5c0ced43de1f0ef9 Mon Sep 17 00:00:00 2001 From: jpokornyiii Date: Wed, 26 Mar 2025 10:50:09 -0400 Subject: [PATCH 15/34] [xrp] Update XRP for new SparkFun RP2350 Board (#7880) Enables servos 3 and 4 usage on the new XRP board. --- simulation/halsim_xrp/src/main/native/cpp/XRP.cpp | 4 ++++ .../src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java | 4 +++- xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp | 4 ++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp index 812ebacb9b..140f696332 100644 --- a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp +++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp @@ -142,6 +142,10 @@ void XRP::HandleServoSimValueChanged(const wpi::json& data) { deviceId = 4; } else if (data["device"] == "servo2") { deviceId = 5; + } else if (data["device"] == "servo3") { + deviceId = 6; + } else if (data["device"] == "servo4") { + deviceId = 7; } if (deviceId != -1 && servoData.find(" XRPServo::s_simDeviceMap = {{4, "servo1"}, - {5, "servo2"}}; +std::map XRPServo::s_simDeviceMap = { + {4, "servo1"}, {5, "servo2"}, {6, "servo3"}, {7, "servo4"}}; std::set XRPServo::s_registeredDevices = {}; From ea986427aa6eac44e97afd919f5be3188d935ff2 Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Thu, 27 Mar 2025 15:51:13 -0700 Subject: [PATCH 16/34] [wpiutil] Add Kaitai Struct definition for data log format (#7882) Adds a Kaitai Struct definition for the WPILOG format. This can be used to generate code to read/write data log files without needing to build out a parser/serializer by hand. Or just an additional resource for readers to help understand the WPILOG format. --- wpiutil/doc/datalog.adoc | 6 ++ wpiutil/doc/wpilog.ksy | 130 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 136 insertions(+) create mode 100644 wpiutil/doc/wpilog.ksy diff --git a/wpiutil/doc/datalog.adoc b/wpiutil/doc/datalog.adoc index 9fd6f6f980..a6ea78c98e 100644 --- a/wpiutil/doc/datalog.adoc +++ b/wpiutil/doc/datalog.adoc @@ -190,3 +190,9 @@ Each entry's data type is an arbitrary string. The following data types are stan === Metadata Each entry has an associated metadata string. If not blank, the metadata should be <>, but may be arbitrary text. Metadata is intended to convey additional information about the entry beyond what the type conveys--for example the source of the data. + +[[additional-resources]] +== Additional Resources + +A https://kaitai.io/[Kaitai Struct] definition for the data log format is included in link:./wpilog.ksy[wpilog.ksy]. +Kaitai Struct is a declarative language used to describe various binary data structures. diff --git a/wpiutil/doc/wpilog.ksy b/wpiutil/doc/wpilog.ksy new file mode 100644 index 0000000000..efba8116cd --- /dev/null +++ b/wpiutil/doc/wpilog.ksy @@ -0,0 +1,130 @@ +meta: + id: wpilog + title: WPILib Data Log + file-extension: wpilog + ks-version: "0.10" + encoding: UTF-8 + endian: le + bit-endian: le +seq: + - id: header + type: header + - id: records + type: record + repeat: eos +types: + version_number: + seq: + - id: minor + type: u1 + - id: major + type: u1 + header: + seq: + - id: magic + contents: WPILOG + - id: version + type: version_number + - id: extra_header_length + type: u4 + - id: extra_header + size: extra_header_length + type: str + record: + seq: + - id: header_length + type: record_header_length + - id: entry_id + type: + switch-on: header_length.len_entry_id + cases: + 0: u1 + 1: u2 + 2: b24 + 3: u4 + - id: len_payload + type: + switch-on: header_length.len_payload_size + cases: + 0: u1 + 1: u2 + 2: b24 + 3: u4 + - id: timestamp + type: + switch-on: header_length.len_timestamp + cases: + 0: u1 + 1: u2 + 2: b24 + 3: u4 + 4: b40 + 5: b48 + 6: b56 + 7: u8 + - id: payload + size: len_payload + type: + switch-on: entry_id + cases: + # ID 0 is reserved for control records + 0: control_record_payload + record_header_length: + seq: + - id: len_entry_id + type: b2 + - id: len_payload_size + type: b2 + - id: len_timestamp + type: b3 + - id: spare_bit + type: b1 + control_record_payload: + seq: + - id: type + type: u1 + enum: control_record_type + - id: data + type: + switch-on: type + cases: + "control_record_type::start": control_record_start_data + "control_record_type::finish": control_record_finish_data + "control_record_type::set_metadata": control_record_set_metadata_data + control_record_start_data: + seq: + - id: entry_id + type: u4 + - id: len_entry_name + type: u4 + - id: entry_name + size: len_entry_name + type: str + - id: len_entry_type + type: u4 + - id: entry_type + size: len_entry_type + type: str + - id: len_entry_metadata + type: u4 + - id: entry_metadata + size: len_entry_metadata + type: str + control_record_finish_data: + seq: + - id: entry_id + type: u4 + control_record_set_metadata_data: + seq: + - id: entry_id + type: u4 + - id: len_entry_metadata + type: u4 + - id: entry_metadata + size: len_entry_metadata + type: str +enums: + control_record_type: + 0: start + 1: finish + 2: set_metadata From c13b2f45c1b0bb78e8d4cf9b6f6bc77b442ef838 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 30 Mar 2025 16:25:08 -0700 Subject: [PATCH 17/34] [wpimath] Update link to Ramsete controller derivation (#7887) The derivation was moved from the Controls Engineering in FRC book to a separate paper. --- .../edu/wpi/first/math/controller/RamseteController.java | 5 ++--- .../main/native/include/frc/controller/RamseteController.h | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java index 78e938fe8b..e9599479ed 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java @@ -28,9 +28,8 @@ import edu.wpi.first.math.trajectory.Trajectory; * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e * Mobile per i SErvizi e le TEcnologie"). * - *

See Controls - * Engineering in the FIRST Robotics Competition section on Ramsete unicycle controller for a - * derivation and analysis. + *

See this paper + * for a derivation and analysis. */ public class RamseteController { private final double m_b; diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h index 1a31bc9ce1..0e6b437cc6 100644 --- a/wpimath/src/main/native/include/frc/controller/RamseteController.h +++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h @@ -42,8 +42,8 @@ namespace frc { * acronym for the title of the book it came from in Italian ("Robotica * Articolata e Mobile per i SErvizi e le TEcnologie"). * - * See section - * on Ramsete unicycle controller for a derivation and analysis. + * See for a + * derivation and analysis. */ class WPILIB_DLLEXPORT RamseteController { public: From bd78215b4303032500fe45fb8771ab0a0c131ba1 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 7 Apr 2025 06:31:31 -0700 Subject: [PATCH 18/34] [glass] Fix compilation error with protobuf 30.1 (#7890) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ``` /home/tav/frc/wpilib/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp: In function ‘void UpdateProtobufValueSource(glass::NetworkTablesModel&, glass::NetworkTablesModel::ValueSource*, const google::protobuf::Message&, std::string_view, int64_t)’: /home/tav/frc/wpilib/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp:363:27: error: no match for ‘operator+’ (operand types are ‘const char [7]’ and ‘google::protobuf::internal::DescriptorStringView’ {aka ‘std::basic_string_view’}) 363 | out->typeStr = "proto:" + desc->full_name(); | ~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~ | | | | const char [7] google::protobuf::internal::DescriptorStringView {aka std::basic_string_view} ``` --- glass/src/libnt/native/cpp/NetworkTables.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glass/src/libnt/native/cpp/NetworkTables.cpp b/glass/src/libnt/native/cpp/NetworkTables.cpp index d661ee5f05..ff7d6ff5a6 100644 --- a/glass/src/libnt/native/cpp/NetworkTables.cpp +++ b/glass/src/libnt/native/cpp/NetworkTables.cpp @@ -360,7 +360,7 @@ static void UpdateProtobufValueSource(NetworkTablesModel& model, const google::protobuf::Message& msg, std::string_view name, int64_t time) { auto desc = msg.GetDescriptor(); - out->typeStr = "proto:" + desc->full_name(); + out->typeStr = fmt::format("proto:{}", desc->full_name()); if (!out->valueChildrenMap || desc->field_count() != static_cast(out->valueChildren.size())) { out->valueChildren.clear(); From 49b4b064cf98c0e367f54ac8719779cc158eba3a Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 8 Apr 2025 07:11:15 -0700 Subject: [PATCH 19/34] [docs] Update readme to say Xcode is required (#7892) The command line tools are insufficient. --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f7d65b0faa..565623105f 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen - C++ compiler - On Linux, install GCC 11 or greater - On Windows, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio) - - On macOS, install the Xcode command-line build tools via `xcode-select --install`. Xcode 13 or later is required. + - On macOS 13.3 or newer, install Xcode 14 or later (the command-line build tools are insufficient). - ARM compiler toolchain - Run `./gradlew installRoboRioToolchain` after cloning this repository - If the WPILib installer was used, this toolchain is already installed From 3cd282d9b0dd1685e71b0ab6cd8a098e851c5e6d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 12 Apr 2025 20:29:09 -0700 Subject: [PATCH 20/34] [ci] Upgrade to wpiformat 2025.33 (#7902) --- .github/workflows/lint-format.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 3929d9cd40..d37a62e7f9 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -36,7 +36,7 @@ jobs: - name: Install wpiformat run: | python -m venv ${{ runner.temp }}/wpiformat - ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51 + ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2025.33 - name: Run run: ${{ runner.temp }}/wpiformat/bin/wpiformat - name: Check output @@ -78,7 +78,7 @@ jobs: - name: Install wpiformat run: | python -m venv ${{ runner.temp }}/wpiformat - ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51 + ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2025.33 - name: Create compile_commands.json run: | ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio From ed94e3af3efbaf83f3a6194a83df1c9d5323e710 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 17 Apr 2025 18:59:10 -0700 Subject: [PATCH 21/34] [ci] Upgrade sccache action (#7911) --- .github/workflows/cmake-android.yml | 2 +- .github/workflows/cmake.yml | 2 +- .github/workflows/sanitizers.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index 46e2b09aab..03c4d1c059 100644 --- a/.github/workflows/cmake-android.yml +++ b/.github/workflows/cmake-android.yml @@ -39,7 +39,7 @@ jobs: java-version: 17 - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.5 + uses: mozilla-actions/sccache-action@v0.0.8 - name: Install dependencies run: sudo apt-get update && sudo apt-get install -y ninja-build diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 921a10bbdb..58c6088a11 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -50,7 +50,7 @@ jobs: uses: lukka/get-cmake@v3.29.3 - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.5 + uses: mozilla-actions/sccache-action@v0.0.8 - uses: actions/checkout@v4 diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index b929190135..7990ebf160 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -36,7 +36,7 @@ jobs: run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-17 libprotobuf-dev protobuf-compiler ninja-build - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.5 + uses: mozilla-actions/sccache-action@v0.0.8 - uses: actions/checkout@v4 From 07192285f65321a2f7363227a2216f09b715252d Mon Sep 17 00:00:00 2001 From: Wispy <101812473+WispySparks@users.noreply.github.com> Date: Thu, 17 Apr 2025 21:00:14 -0500 Subject: [PATCH 22/34] [wpiunits] Add InchesPerSecondPerSecond (#7905) --- wpiunits/src/main/java/edu/wpi/first/units/Units.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Units.java b/wpiunits/src/main/java/edu/wpi/first/units/Units.java index d75eebc303..6397c8ca3d 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/Units.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/Units.java @@ -203,6 +203,12 @@ public final class Units { */ public static final LinearAccelerationUnit FeetPerSecondPerSecond = FeetPerSecond.per(Second); + /** + * A unit of linear acceleration equivalent to accelerating at a rate of one {@link #Inch Inch} + * per {@link #Second} every second. + */ + public static final LinearAccelerationUnit InchesPerSecondPerSecond = InchesPerSecond.per(Second); + /** * A unit of angular acceleration equivalent to accelerating at a rate of one {@link #Rotations * Rotation} per {@link #Second} every second. From 26e299115fab948178d0e884af6a2c4d517ae4a3 Mon Sep 17 00:00:00 2001 From: Yuhao Date: Tue, 22 Apr 2025 00:27:42 +0800 Subject: [PATCH 23/34] [cscore] Fix USB video mode handling on macOS (#7904) * fix: UsbCameraImpl on macOS * fix: add back logs --- cscore/src/main/native/objcpp/UsbCameraImpl.h | 1 - .../main/native/objcpp/UsbCameraImplObjc.mm | 27 +++++++++++-------- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/cscore/src/main/native/objcpp/UsbCameraImpl.h b/cscore/src/main/native/objcpp/UsbCameraImpl.h index 0fd09fb7e1..86b1166b05 100644 --- a/cscore/src/main/native/objcpp/UsbCameraImpl.h +++ b/cscore/src/main/native/objcpp/UsbCameraImpl.h @@ -91,6 +91,5 @@ class UsbCameraImpl : public SourceImpl { private: UsbCameraImplObjc* m_objc; std::vector m_platformModes; - VideoMode m_mode; }; } // namespace cs diff --git a/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm b/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm index 0e9d65fb4c..5d43de7f32 100644 --- a/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm +++ b/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm @@ -380,22 +380,27 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) { toCheck->height, toCheck->fps); std::vector& platformModes = sharedThis->objcGetPlatformVideoModes(); - // Find the matching mode - auto match = std::find_if(platformModes.begin(), platformModes.end(), - [&](CameraModeStore& input) { - return input.mode.CompareWithoutFps(*toCheck); - }); + + // Find all matching modes + std::vector matchingModes; + for (auto& mode : platformModes) { + if (mode.mode.CompareWithoutFps(*toCheck)) { + matchingModes.push_back(&mode); + } + } - if (match == platformModes.end()) { + if (matchingModes.empty()) { return nil; } // Check FPS - for (CameraFPSRange& range : match->fpsRanges) { - OBJCDEBUG3("Checking Range {} {}", range.min, range.max); - if (range.IsWithinRange(toCheck->fps)) { - *fps = toCheck->fps; - return match->format; + for (auto mode : matchingModes) { + for (CameraFPSRange& range : mode->fpsRanges) { + OBJCDEBUG3("Checking Range {} {}", range.min, range.max); + if (range.IsWithinRange(toCheck->fps)) { + *fps = toCheck->fps; + return mode->format; + } } } From 21d921184a67a2f3dfc163a41118aa3e83333013 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Tue, 22 Apr 2025 14:26:26 -0700 Subject: [PATCH 24/34] [examples] Add compilable code snippets (#7909) This enables frc-docs to use RLIs for things that are currently in-line code blocks, and ensures they compile, which is important with the 2027 breaking changes coming. They are kept separate from the examples to ensure they don't polute the VSCode examples finder. Adds the Encoder snippets used in the frc-docs Encoder article as the first instance of this. --- shared/examplecheck.gradle | 19 +++ wpilibcExamples/CMakeLists.txt | 14 ++ wpilibcExamples/build.gradle | 123 ++++++++++++++++-- .../main/cpp/snippets/Encoder/cpp/Robot.cpp | 69 ++++++++++ .../src/main/cpp/snippets/snippets.json | 12 ++ wpilibjExamples/build.gradle | 43 ++++++ .../first/wpilibj/snippets/encoder/Main.java | 25 ++++ .../first/wpilibj/snippets/encoder/Robot.java | 62 +++++++++ .../wpi/first/wpilibj/snippets/snippets.json | 13 ++ 9 files changed, 366 insertions(+), 14 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/snippets/snippets.json create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 644c095cf2..27d90762de 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -103,6 +103,24 @@ task checkExamples(type: Task) { } } +task checkSnippets(type: Task) { + doLast { + def parsedJson = new groovy.json.JsonSlurper().parseText(snippetsFile.text) + fileCheck(parsedJson, snippetsDirectory) + parsedJson.each { + assert it.name != null + assert it.description != null + assert it.tags != null + assert it.tags.findAll { !tagList.contains(it) }.empty + assert it.foldername != null + assert it.gradlebase != null + if (it.gradlebase == 'java') { + assert it.mainclass != null + } + } + } +} + task checkCommands(type: Task) { doLast { def parsedJson = new groovy.json.JsonSlurper().parseText(commandFile.text) @@ -127,3 +145,4 @@ task checkCommands(type: Task) { check.dependsOn checkTemplates check.dependsOn checkExamples check.dependsOn checkCommands +check.dependsOn checkSnippets diff --git a/wpilibcExamples/CMakeLists.txt b/wpilibcExamples/CMakeLists.txt index aaec73c88f..f79cd15d68 100644 --- a/wpilibcExamples/CMakeLists.txt +++ b/wpilibcExamples/CMakeLists.txt @@ -5,6 +5,7 @@ include(SubDirList) subdir_list(TEMPLATES ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/templates) subdir_list(EXAMPLES ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/examples) +subdir_list(SNIPPETS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/snippets) add_custom_target(wpilibcExamples) add_custom_target(wpilibcExamples_test) @@ -63,3 +64,16 @@ foreach(template ${TEMPLATES}) target_link_libraries(${template} wpilibc wpilibNewCommands romiVendordep xrpVendordep) add_dependencies(wpilibcExamples ${template}) endforeach() + +foreach(snippet ${SNIPPETS}) + file( + GLOB_RECURSE sources + src/main/cpp/snippets/${snippet}/cpp/*.cpp + src/main/cpp/snippets/${snippet}/c/*.c + ) + add_executable(snippet${snippet} ${sources}) + wpilib_target_warnings(${snippet}) + target_include_directories(snippet${snippet} PUBLIC src/main/cpp/snippets/${snippet}/include) + target_link_libraries(snippet${snippet} wpilibc wpilibNewCommands romiVendordep xrpVendordep) + add_dependencies(wpilibcExamples snippet${snippet}) +endforeach() diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle index eb1aca5251..aa0aaedef4 100644 --- a/wpilibcExamples/build.gradle +++ b/wpilibcExamples/build.gradle @@ -14,9 +14,20 @@ apply from: "${rootDir}/shared/googletest.gradle" ext.examplesMap = [:] ext.templatesMap = [:] +ext.snippetsMap = [:] -File examplesTree = file("$projectDir/src/main/cpp/examples") -examplesTree.list(new FilenameFilter() { +ext { + templateDirectory = new File("$projectDir/src/main/cpp/templates/") + templateFile = new File("$projectDir/src/main/cpp/templates/templates.json") + exampleDirectory = new File("$projectDir/src/main/cpp/examples/") + exampleFile = new File("$projectDir/src/main/cpp/examples/examples.json") + commandDirectory = new File("$projectDir/src/main/cpp/commands/") + commandFile = new File("$projectDir/src/main/cpp/commands/commands.json") + snippetsDirectory = new File("$projectDir/src/main/cpp/snippets/") + snippetsFile = new File("$projectDir/src/main/cpp/snippets/snippets.json") +} + +exampleDirectory.list(new FilenameFilter() { @Override public boolean accept(File current, String name) { return new File(current, name).isDirectory(); @@ -24,8 +35,7 @@ examplesTree.list(new FilenameFilter() { }).each { examplesMap.put(it, []) } -File templatesTree = file("$projectDir/src/main/cpp/templates") -templatesTree.list(new FilenameFilter() { +templateDirectory.list(new FilenameFilter() { @Override public boolean accept(File current, String name) { return new File(current, name).isDirectory(); @@ -33,6 +43,14 @@ templatesTree.list(new FilenameFilter() { }).each { templatesMap.put(it, []) } +snippetsDirectory.list(new FilenameFilter() { + @Override + public boolean accept(File current, String name) { + return new File(current, name).isDirectory(); + } + }).each { + snippetsMap.put(it, []) + } nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.windowsx64).configure { linker.args.remove('/DEBUG:FULL') @@ -41,7 +59,7 @@ nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.windowsx64).configur } ext { - sharedCvConfigs = examplesMap + templatesMap + [commands: []] + sharedCvConfigs = examplesMap + templatesMap + snippetsMap.collectEntries { key, value -> ['snippets' + key, value] } + [commands: []] staticCvConfigs = [:] useJava = false useCpp = true @@ -171,6 +189,57 @@ model { } } } + snippetsMap.each { key, value -> + "snippets${key}"(NativeExecutableSpec) { + targetBuildTypes 'debug' + binaries.all { binary -> + lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared' + lib project: ':romiVendordep', library: 'romiVendordep', linkage: 'shared' + lib project: ':xrpVendordep', library: 'xrpVendordep', linkage: 'shared' + lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared' + lib project: ':apriltag', library: 'apriltag', linkage: 'shared' + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + project(':ntcore').addNtcoreDependency(binary, 'shared') + lib project: ':cscore', library: 'cscore', linkage: 'shared' + project(':hal').addHalDependency(binary, 'shared') + lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared' + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries') + } + if (binary.targetPlatform.name == getCurrentArch()) { + simModules.each { + lib project: ":simulation:$it", library: it, linkage: 'shared' + } + } + } + sources { + cpp { + source { + srcDirs 'src/main/cpp/snippets/' + "${key}" + "/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/cpp/snippets/' + "${key}" + "/include" + include '**/*.h' + } + } + } + sources { + c { + source { + srcDirs 'src/main/cpp/snippets/' + "${key}" + "/c" + include '**/*.c' + } + exportedHeaders { + srcDirs 'src/main/cpp/snippets/' + "${key}" + "/include" + include '**/*.h' + } + } + } + } + } } testSuites { examplesMap.each { key, value -> @@ -207,6 +276,41 @@ model { } } } + testSuites { + snippetsMap.each { key, value -> + def testFolder = new File("${rootDir}/wpilibcExamples/src/test/cpp/snippets/${key}") + if (testFolder.exists()) { + "snippets${key}Test"(GoogleTestTestSuiteSpec) { + for (NativeComponentSpec c : $.components) { + if (c.name == key) { + testing c + break + } + } + sources { + cpp { + source { + srcDirs "src/test/cpp/examples/${key}/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs "src/test/cpp/examples/${key}/include" + } + } + c { + source { + srcDirs "src/test/cpp/examples/${key}/c" + include '**/*.c' + } + exportedHeaders { + srcDirs "src/test/cpp/examples/${key}/include" + } + } + } + } + } + } + } binaries { withType(GoogleTestTestSuiteBinarySpec) { lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared' @@ -253,15 +357,6 @@ model { } } -ext { - templateDirectory = new File("$projectDir/src/main/cpp/templates/") - templateFile = new File("$projectDir/src/main/cpp/templates/templates.json") - exampleDirectory = new File("$projectDir/src/main/cpp/examples/") - exampleFile = new File("$projectDir/src/main/cpp/examples/examples.json") - commandDirectory = new File("$projectDir/src/main/cpp/commands/") - commandFile = new File("$projectDir/src/main/cpp/commands/commands.json") -} - model { // Create run tasks for all examples. tasks { diff --git a/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp new file mode 100644 index 0000000000..7f736d5c1b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include +#include +#include +#include + +/** + * Encoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +WPI_IGNORE_DEPRECATED +class Robot : public frc::TimedRobot { + public: + Robot() { + // Configures the encoder to return a distance of 4 for every 256 pulses + // Also changes the units of getRate + m_encoder.SetDistancePerPulse(4.0 / 256.0); + // Configures the encoder to consider itself stopped after .1 seconds + m_encoder.SetMaxPeriod(0.1_s); + // Configures the encoder to consider itself stopped when its rate is below + // 10 + m_encoder.SetMinRate(10); + // Reverses the direction of the encoder + m_encoder.SetReverseDirection(true); + // Configures an encoder to average its period measurement over 5 samples + // Can be between 1 and 127 samples + m_encoder.SetSamplesToAverage(5); + } + + void TeleopPeriodic() override { + // Gets the distance traveled + m_encoder.GetDistance(); + + // Gets the current rate of the encoder + m_encoder.GetRate(); + + // Gets whether the encoder is stopped + m_encoder.GetStopped(); + + // Gets the last direction in which the encoder moved + m_encoder.GetDirection(); + + // Gets the current period of the encoder + m_encoder.GetPeriod(); + + // Resets the encoder to read a distance of zero + m_encoder.Reset(); + } + + private: + // Initializes an encoder on DIO pins 0 and 1 + // Defaults to 4X decoding and non-inverted + frc::Encoder m_encoder{0, 1}; + + // Initializes an encoder on DIO pins 0 and 1 + // 2X encoding and non-inverted + frc::Encoder m_encoder2x{0, 1, false, frc::Encoder::EncodingType::k2X}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json new file mode 100644 index 0000000000..884e075dfd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -0,0 +1,12 @@ +[ + { + "name": "Encoder", + "description": "Snippets of Encoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder" + ], + "foldername": "Encoder", + "gradlebase": "cpp" + } +] diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle index 115b253965..eec38c0a35 100644 --- a/wpilibjExamples/build.gradle +++ b/wpilibjExamples/build.gradle @@ -72,6 +72,8 @@ ext { exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json") commandDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/") commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json") + snippetsDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/snippets/") + snippetsFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json") } apply plugin: 'cpp' @@ -203,6 +205,47 @@ model { test.dependsOn(testTask) } } + new groovy.json.JsonSlurper().parseText(snippetsFile.text).each { entry -> + project.tasks.create("runSnippet${entry.foldername}", JavaExec) { run -> + run.group "run snippets" + run.mainClass = "edu.wpi.first.wpilibj.snippets." + entry.foldername + "." + entry.mainclass + run.classpath = sourceSets.main.runtimeClasspath + run.dependsOn it.tasks.install + run.systemProperty 'java.library.path', filePath + doFirst { doFirstTask(run) } + + if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) { + run.jvmArgs = ['-XstartOnFirstThread'] + } + } + project.tasks.create("testSnippet${entry.foldername}", Test) { testTask -> + testTask.group "verification" + testTask.useJUnitPlatform() + testTask.filter { + includeTestsMatching("edu.wpi.first.wpilibj.snippets.${entry.foldername}.*") + setFailOnNoMatchingTests(false) + } + test.filter { + excludeTestsMatching("edu.wpi.first.wpilibj.snippets.${entry.foldername}.*") + setFailOnNoMatchingTests(false) + } + testTask.testClassesDirs = sourceSets.test.output.classesDirs + testTask.classpath = sourceSets.test.runtimeClasspath + testTask.dependsOn it.tasks.install + + testTask.systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + testTask.testLogging { + events "failed" + exceptionFormat "full" + } + testTask.systemProperty 'java.library.path', filePath + + if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { + testTask.enabled = false + } + test.dependsOn(testTask) + } + } found = true } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java new file mode 100644 index 0000000000..4fef5feb29 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoder; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.java new file mode 100644 index 0000000000..de1511bd55 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoder/Robot.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoder; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * Encoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +@SuppressWarnings("deprecation") +public class Robot extends TimedRobot { + // Initializes an encoder on DIO pins 0 and 1 + // Defaults to 4X decoding and non-inverted + Encoder m_encoder = new Encoder(0, 1); + + // Initializes an encoder on DIO pins 0 and 1 + // 2X encoding and non-inverted + Encoder m_encoder2x = new Encoder(0, 1, false, Encoder.EncodingType.k2X); + + /** Called once at the beginning of the robot program. */ + public Robot() { + // Configures the encoder to return a distance of 4 for every 256 pulses + // Also changes the units of getRate + m_encoder.setDistancePerPulse(4.0 / 256.0); + // Configures the encoder to consider itself stopped after .1 seconds + m_encoder.setMaxPeriod(0.1); + // Configures the encoder to consider itself stopped when its rate is below 10 + m_encoder.setMinRate(10); + // Reverses the direction of the encoder + m_encoder.setReverseDirection(true); + // Configures an encoder to average its period measurement over 5 samples + // Can be between 1 and 127 samples + m_encoder.setSamplesToAverage(5); + + m_encoder2x.getRate(); + } + + @Override + public void teleopPeriodic() { + // Gets the distance traveled + m_encoder.getDistance(); + + // Gets the current rate of the encoder + m_encoder.getRate(); + + // Gets whether the encoder is stopped + m_encoder.getStopped(); + + // Gets the last direction in which the encoder moved + m_encoder.getDirection(); + + // Gets the current period of the encoder + m_encoder.getPeriod(); + + // Resets the encoder to read a distance of zero + m_encoder.reset(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json new file mode 100644 index 0000000000..8e8786f713 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -0,0 +1,13 @@ +[ + { + "name": "Encoder", + "description": "Snippets of Encoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder" + ], + "foldername": "encoder", + "gradlebase": "java", + "mainclass": "Main" + } +] From 9c60da13198a04e22c4102b519a6185ea428cd9f Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 22 Apr 2025 21:27:20 +0000 Subject: [PATCH 25/34] [ci] Use sccache for RobotPy build (#7915) Also uses Artifactory to store the caches. --- .github/workflows/tools.yml | 24 +++++++----------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/.github/workflows/tools.yml b/.github/workflows/tools.yml index b2b0a91d1a..5137b0c549 100644 --- a/.github/workflows/tools.yml +++ b/.github/workflows/tools.yml @@ -169,22 +169,8 @@ jobs: with: python-version: 3.13 - # - # Setup build caching - # - - - name: Set ccache size - shell: bash - id: ccache - run: echo "MAX_SIZE=500M" >> $GITHUB_OUTPUT - - - name: Setup ccache - # uses: hendrikmuhs/ccache-action@v1.2.10 - uses: robotpy/ccache-action@fork - with: - key: ubuntu-22.04-3.13 - variant: ccache - max-size: ${{ steps.ccache.outputs.max_size }} + - name: Install sccache + uses: mozilla-actions/sccache-action@v0.0.9 - name: Install deps shell: bash @@ -203,4 +189,8 @@ jobs: ./rdev.sh ci run env: RPYBUILD_STRIP_LIBPYTHON: "1" - RPYBUILD_CC_LAUNCHER: ccache + RPYBUILD_CC_LAUNCHER: sccache + SCCACHE_WEBDAV_ENDPOINT: "https://frcmaven.wpi.edu/artifactory/wpilib-generic-cache-cmake-local" + SCCACHE_WEBDAV_KEY_PREFIX: "sccache-robotpy" + SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }} + SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }} From b2486633862f86092354df5c1bd92f5db457afcb Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 22 Apr 2025 21:27:55 +0000 Subject: [PATCH 26/34] [ci] Upgrade sccache action (#7914) --- .github/workflows/cmake-android.yml | 2 +- .github/workflows/cmake.yml | 2 +- .github/workflows/sanitizers.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index 03c4d1c059..338eedee1f 100644 --- a/.github/workflows/cmake-android.yml +++ b/.github/workflows/cmake-android.yml @@ -39,7 +39,7 @@ jobs: java-version: 17 - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.8 + uses: mozilla-actions/sccache-action@v0.0.9 - name: Install dependencies run: sudo apt-get update && sudo apt-get install -y ninja-build diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 58c6088a11..821662548a 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -50,7 +50,7 @@ jobs: uses: lukka/get-cmake@v3.29.3 - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.8 + uses: mozilla-actions/sccache-action@v0.0.9 - uses: actions/checkout@v4 diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index 7990ebf160..2dbb89fa74 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -36,7 +36,7 @@ jobs: run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-17 libprotobuf-dev protobuf-compiler ninja-build - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.8 + uses: mozilla-actions/sccache-action@v0.0.9 - uses: actions/checkout@v4 From 1c35a3a5ff9a5bd2ef0f9a1309d741de77f0b695 Mon Sep 17 00:00:00 2001 From: Austin Schuh Date: Wed, 23 Apr 2025 07:31:43 -0700 Subject: [PATCH 27/34] [bazel] Make bazel work outside // (#7918) The .bazelrc was doing a relative import, not an absolute one. Signed-off-by: Austin Schuh --- .bazelrc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.bazelrc b/.bazelrc index aad909f686..753c0d0b82 100644 --- a/.bazelrc +++ b/.bazelrc @@ -11,14 +11,14 @@ build --tool_java_runtime_version=remotejdk_17 test --test_output=errors test --test_verbose_timeout_warnings -import shared/bazel/compiler_flags/sanitizers.rc -import shared/bazel/compiler_flags/base_linux_flags.rc -import shared/bazel/compiler_flags/linux_flags.rc -import shared/bazel/compiler_flags/osx_flags.rc -import shared/bazel/compiler_flags/roborio_flags.rc -import shared/bazel/compiler_flags/systemcore_flags.rc -import shared/bazel/compiler_flags/windows_flags.rc -import shared/bazel/compiler_flags/coverage_flags.rc +import %workspace%/shared/bazel/compiler_flags/sanitizers.rc +import %workspace%/shared/bazel/compiler_flags/base_linux_flags.rc +import %workspace%/shared/bazel/compiler_flags/linux_flags.rc +import %workspace%/shared/bazel/compiler_flags/osx_flags.rc +import %workspace%/shared/bazel/compiler_flags/roborio_flags.rc +import %workspace%/shared/bazel/compiler_flags/systemcore_flags.rc +import %workspace%/shared/bazel/compiler_flags/windows_flags.rc +import %workspace%/shared/bazel/compiler_flags/coverage_flags.rc # Alias toolchain names to what wpilibsuite uses for CI/Artifact naming build:athena --config=roborio From d98ad815b191ff5296e9c5eceeee065f0888d8b1 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 23 Apr 2025 09:41:03 -0500 Subject: [PATCH 28/34] [wpimath] Adjust applyDeadband logic (#7816) Reduces nesting by returning when the value is within the deadband. Adjusts the algorithm to handle large values of maxMagnitude naturally (instead of needing a separate check). Reformats the math comments. --- .../java/edu/wpi/first/math/MathUtil.java | 87 +++++++++--------- .../src/main/native/include/frc/MathUtil.h | 90 ++++++++++--------- 2 files changed, 91 insertions(+), 86 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 1836005cee..1e374ec583 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -47,50 +47,51 @@ public final class MathUtil { * @return The value after the deadband is applied. */ public static double applyDeadband(double value, double deadband, double maxMagnitude) { - if (Math.abs(value) > deadband) { - if (maxMagnitude / deadband > 1.0e12) { - // If max magnitude is sufficiently large, the implementation encounters - // roundoff error. Implementing the limiting behavior directly avoids - // the problem. - return value > 0.0 ? value - deadband : value + deadband; - } - if (value > 0.0) { - // Map deadband to 0 and map max to max. - // - // y - y₁ = m(x - x₁) - // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) - // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ - // - // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). - // x₁ = deadband - // y₁ = 0 - // x₂ = max - // y₂ = max - // - // y = (max - 0)/(max - deadband) (x - deadband) + 0 - // y = max/(max - deadband) (x - deadband) - // y = max (x - deadband)/(max - deadband) - return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); - } else { - // Map -deadband to 0 and map -max to -max. - // - // y - y₁ = m(x - x₁) - // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) - // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ - // - // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). - // x₁ = -deadband - // y₁ = 0 - // x₂ = -max - // y₂ = -max - // - // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 - // y = max/(max - deadband) (x + deadband) - // y = max (x + deadband)/(max - deadband) - return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); - } + if (Math.abs(value) < deadband) { + return 0; + } + if (value > 0.0) { + // Map deadband to 0 and map max to max with a linear relationship. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). + // + // x₁ = deadband + // y₁ = 0 + // x₂ = max + // y₂ = max + // y = (max - 0)/(max - deadband) (x - deadband) + 0 + // y = max/(max - deadband) (x - deadband) + // + // To handle high values of max, rewrite so that max only appears on the denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x - deadband) + // y = (1 + deadband/(max - deadband)) (x - deadband) + return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband); } else { - return 0.0; + // Map -deadband to 0 and map -max to -max with a linear relationship. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). + // + // x₁ = -deadband + // y₁ = 0 + // x₂ = -max + // y₂ = -max + // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 + // y = max/(max - deadband) (x + deadband) + // + // To handle high values of max, rewrite so that max only appears on the denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x + deadband) + // y = (1 + deadband/(max - deadband)) (x + deadband) + return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband); } } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index e6ec32908d..060be0da91 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -43,51 +43,55 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { magnitude = units::math::abs(value); } - if (magnitude > deadband) { - if (maxMagnitude / deadband > 1.0E12) { - // If max magnitude is sufficiently large, the implementation encounters - // roundoff error. Implementing the limiting behavior directly avoids - // the problem. - return value > T{0.0} ? value - deadband : value + deadband; - } - if (value > T{0.0}) { - // Map deadband to 0 and map max to max. - // - // y - y₁ = m(x - x₁) - // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) - // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ - // - // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). - // x₁ = deadband - // y₁ = 0 - // x₂ = max - // y₂ = max - // - // y = (max - 0)/(max - deadband) (x - deadband) + 0 - // y = max/(max - deadband) (x - deadband) - // y = max (x - deadband)/(max - deadband) - return maxMagnitude * (value - deadband) / (maxMagnitude - deadband); - } else { - // Map -deadband to 0 and map -max to -max. - // - // y - y₁ = m(x - x₁) - // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) - // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ - // - // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). - // x₁ = -deadband - // y₁ = 0 - // x₂ = -max - // y₂ = -max - // - // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 - // y = max/(max - deadband) (x + deadband) - // y = max (x + deadband)/(max - deadband) - return maxMagnitude * (value + deadband) / (maxMagnitude - deadband); - } - } else { + if (magnitude < deadband) { return T{0.0}; } + + if (value > T{0.0}) { + // Map deadband to 0 and map max to max with a linear relationship. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max). + // + // x₁ = deadband + // y₁ = 0 + // x₂ = max + // y₂ = max + // y = (max - 0)/(max - deadband) (x - deadband) + 0 + // y = max/(max - deadband) (x - deadband) + // + // To handle high values of max, rewrite so that max only appears on the + // denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x - deadband) + // y = (1 + deadband/(max - deadband)) (x - deadband) + return (1.0 + deadband / (maxMagnitude - deadband)) * (value - deadband); + } else { + // Map -deadband to 0 and map -max to -max with a linear relationship. + // + // y - y₁ = m(x - x₁) + // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁ + // + // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max). + // + // x₁ = -deadband + // y₁ = 0 + // x₂ = -max + // y₂ = -max + // y = (-max - 0)/(-max + deadband) (x + deadband) + 0 + // y = max/(max - deadband) (x + deadband) + // + // To handle high values of max, rewrite so that max only appears on the + // denominator. + // + // y = ((max - deadband) + deadband)/(max - deadband) (x + deadband) + // y = (1 + deadband/(max - deadband)) (x + deadband) + return (1.0 + deadband / (maxMagnitude - deadband)) * (value + deadband); + } } /** From d04e15b957bd59f7f9b3a4d3433e567505c43045 Mon Sep 17 00:00:00 2001 From: David Vo Date: Sat, 26 Apr 2025 03:37:42 +1000 Subject: [PATCH 29/34] [bazel] Avoid globally linking macOS SDK frameworks (#7927) --- cscore/BUILD.bazel | 7 +++++++ shared/bazel/compiler_flags/osx_flags.rc | 11 ----------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/cscore/BUILD.bazel b/cscore/BUILD.bazel index d40fb4d2fd..8533fde451 100644 --- a/cscore/BUILD.bazel +++ b/cscore/BUILD.bazel @@ -40,6 +40,13 @@ objc_library( "src/main/native/include", "src/main/native/objcpp", ], + sdk_frameworks = [ + "CoreFoundation", + "AVFoundation", + "Foundation", + "CoreMedia", + "CoreVideo", + ], tags = ["manual"], deps = [ "//wpinet:wpinet.static", diff --git a/shared/bazel/compiler_flags/osx_flags.rc b/shared/bazel/compiler_flags/osx_flags.rc index 2aa48df280..fd71773579 100644 --- a/shared/bazel/compiler_flags/osx_flags.rc +++ b/shared/bazel/compiler_flags/osx_flags.rc @@ -26,17 +26,6 @@ build:macos --conlyopt=-Wno-missing-field-initializers build:macos --conlyopt=-Wno-unused-private-field build:macos --conlyopt=-Wno-fixed-enum-extension" - -build:macos --linkopt=-framework -build:macos --linkopt=CoreFoundation -build:macos --linkopt=-framework -build:macos --linkopt=AVFoundation -build:macos --linkopt=-framework -build:macos --linkopt=Foundation -build:macos --linkopt=-framework -build:macos --linkopt=CoreMedia -build:macos --linkopt=-framework -build:macos --linkopt=CoreVideo build:macos --linkopt=-headerpad_max_install_names build:macos --linkopt=-Wl,-rpath,'@loader_path'" From 828199befb6b3ef2e819f1fb1d66a364029db22d Mon Sep 17 00:00:00 2001 From: David Vo Date: Sat, 26 Apr 2025 14:46:40 +1000 Subject: [PATCH 30/34] [ci] Use Bazel fastbuild compilation mode on Windows (#7925) --- .github/workflows/bazel.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/bazel.yml b/.github/workflows/bazel.yml index b85222db9b..3b93739f3a 100644 --- a/.github/workflows/bazel.yml +++ b/.github/workflows/bazel.yml @@ -31,8 +31,8 @@ jobs: with: token: ${{ secrets.BUILDBUDDY_API_KEY }} - - name: Build Release - run: bazel --output_user_root=C:\\bazelroot ${{ matrix.action }} -k ... --config=ci -c opt ${{ matrix.config }} --verbose_failures + - name: bazel ${{ matrix.action }} + run: bazel --output_user_root=C:\\bazelroot ${{ matrix.action }} -k ... --config=ci ${{ matrix.config }} --verbose_failures shell: bash build-mac: @@ -47,7 +47,7 @@ jobs: with: token: ${{ secrets.BUILDBUDDY_API_KEY }} - - name: Build Release + - name: bazel test (release) run: bazel test -k ... --config=ci -c opt --config=macos --nojava_header_compilation --verbose_failures shell: bash @@ -70,7 +70,7 @@ jobs: with: token: ${{ secrets.BUILDBUDDY_API_KEY }} - - name: Build and Test Release + - name: bazel ${{ matrix.action }} (release) run: bazel ${{ matrix.action }} ... --config=ci -c opt ${{ matrix.config }} -k --verbose_failures buildifier: From 90ee11a9e0543fb4abb88ad34aab6e0e877ffce8 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Fri, 25 Apr 2025 21:47:53 -0700 Subject: [PATCH 31/34] [examples] Add DutyCycleEncoder Snippets (#7930) --- .../snippets/DutyCycleEncoder/cpp/Robot.cpp | 40 +++++++++++++++++++ .../src/main/cpp/snippets/snippets.json | 11 +++++ .../snippets/dutycycleencoder/Main.java | 25 ++++++++++++ .../snippets/dutycycleencoder/Robot.java | 36 +++++++++++++++++ .../wpi/first/wpilibj/snippets/snippets.json | 12 ++++++ 5 files changed, 124 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java diff --git a/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp new file mode 100644 index 0000000000..25593ec933 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include +#include + +/** + * DutyCycleEncoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() {} + + void TeleopPeriodic() override { + // Gets the rotation + m_encoder.Get(); + + // Gets if the encoder is connected + m_encoder.IsConnected(); + } + + private: + // Initializes a duty cycle encoder on DIO pins 0 + frc::DutyCycleEncoder m_encoder{0}; + + // Initializes a duty cycle encoder on DIO pins 0 to return a value of 4 for + // a full rotation, with the encoder reporting 0 half way through rotation (2 + // out of 4) + frc::DutyCycleEncoder m_encoderFR{0, 4.0, 2.0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index 884e075dfd..5edd312bb9 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -8,5 +8,16 @@ ], "foldername": "Encoder", "gradlebase": "cpp" + }, + { + "name": "DutyCycleEncoder", + "description": "Snippets of DutyCycleEncoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Duty Cycle" + ], + "foldername": "DutyCycleEncoder", + "gradlebase": "cpp" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java new file mode 100644 index 0000000000..add47abd4d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.dutycycleencoder; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java new file mode 100644 index 0000000000..ed7e3fe89b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/dutycycleencoder/Robot.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.dutycycleencoder; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * DutyCycleEncoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +public class Robot extends TimedRobot { + // Initializes a duty cycle encoder on DIO pins 0 + DutyCycleEncoder m_encoder = new DutyCycleEncoder(0); + + // Initializes a duty cycle encoder on DIO pins 0 to return a value of 4 for + // a full rotation, with the encoder reporting 0 half way through rotation (2 + // out of 4) + DutyCycleEncoder m_encoderFR = new DutyCycleEncoder(0, 4.0, 2.0); + + /** Called once at the beginning of the robot program. */ + public Robot() {} + + @Override + public void teleopPeriodic() { + // Gets the rotation + m_encoder.get(); + + // Gets if the encoder is connected + m_encoder.isConnected(); + + m_encoderFR.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json index 8e8786f713..abc1bd45e1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -9,5 +9,17 @@ "foldername": "encoder", "gradlebase": "java", "mainclass": "Main" + }, + { + "name": "DutyCycleEncoder", + "description": "Snippets of DutyCycleEncoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Duty Cycle" + ], + "foldername": "dutycycleencoder", + "gradlebase": "java", + "mainclass": "Main" } ] From 92010c175f392f530fdcdf8e6f6407cdfe332375 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 25 Apr 2025 21:56:26 -0700 Subject: [PATCH 32/34] Fix more emscripten compiler errors (#7895) I ran the CMake configure with: ```bash emcmake cmake -B build-wasm -S . \ -DCMAKE_BUILD_TYPE=Release \ -DBUILD_SHARED_LIBS=OFF \ -DWITH_SIMULATION_MODULES=OFF \ -DWITH_PROTOBUF=OFF \ -DWITH_GUI=OFF \ -DWITH_CSCORE=OFF ``` * Turned off simulation modules because they require shared libraries * Turned off GUI because glfw requires libssh * Turned off cscore because it requires OpenCV I still get the following compiler errors: ``` /home/tav/frc/wpilib/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp:43:10: fatal error: 'sys/epoll.h' file not found 43 | #include | ^~~~~~~~~~~~~ ``` ``` /home/tav/frc/wpilib/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/stream.cpp:991:56: error: comparison of integers of different signs: 'unsigned long' and 'long' [-Werror,-Wsign-compare] 991 | for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; cmsg = CMSG_NXTHDR(msg, cmsg)) { | ^~~~~~~~~~~~~~~~~~~~~~ /home/tav/.cache/emscripten/sysroot/include/sys/socket.h:358:44: note: expanded from macro 'CMSG_NXTHDR' 358 | __CMSG_LEN(cmsg) + sizeof(struct cmsghdr) >= __MHDR_END(mhdr) - (unsigned char *)(cmsg) \ | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ``` ``` /home/tav/frc/wpilib/allwpilib/wpinet/src/main/native/thirdparty/libuv/src/unix/core.cpp:748:56: error: comparison of integers of different signs: 'unsigned long' and 'long' [-Werror,-Wsign-compare] 748 | for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; cmsg = CMSG_NXTHDR(msg, cmsg)) | ^~~~~~~~~~~~~~~~~~~~~~ /home/tav/.cache/emscripten/sysroot/include/sys/socket.h:358:44: note: expanded from macro 'CMSG_NXTHDR' 358 | __CMSG_LEN(cmsg) + sizeof(struct cmsghdr) >= __MHDR_END(mhdr) - (unsigned char *)(cmsg) \ | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ``` --- cmake/modules/CompileWarnings.cmake | 2 +- ntcore/src/test/native/cpp/ValueTest.cpp | 3 ++ .../0001-Guard-gnu-flatten-attribute.patch | 2 +- ...query-argument-from-Windows-and-macO.patch | 2 +- ...ix-exception-mask-type-typo-on-macOS.patch | 2 +- ...4-Remove-NOMINMAX-macro-from-Windows.patch | 2 +- .../0005-Add-emscripten-shim.patch | 34 ++++++++++++ ...001-Group-doxygen-into-memory-module.patch | 2 +- ...ve-conflicting-doxygen-concept-alias.patch | 2 +- ...003-Fix-deprecation-warning-for-UDLs.patch | 52 +++++++++++++++++++ .../thirdparty/debugging/src/emscripten.cpp | 18 +++++++ .../include/wpi/memory/memory_arena.hpp | 12 ++--- .../src/test/native/include/wpi/SpanMatcher.h | 2 +- 13 files changed, 121 insertions(+), 14 deletions(-) create mode 100644 upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch create mode 100644 upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch create mode 100644 wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp diff --git a/cmake/modules/CompileWarnings.cmake b/cmake/modules/CompileWarnings.cmake index cadc9fe02e..5e57d292c9 100644 --- a/cmake/modules/CompileWarnings.cmake +++ b/cmake/modules/CompileWarnings.cmake @@ -46,7 +46,7 @@ macro(wpilib_target_warnings target) # Suppress warning "enumeration types with a fixed underlying type are a # Clang extension" - if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT EMSCRIPTEN) target_compile_options(${target} PRIVATE $<$:-Wno-fixed-enum-extension>) endif() diff --git a/ntcore/src/test/native/cpp/ValueTest.cpp b/ntcore/src/test/native/cpp/ValueTest.cpp index c45d5aafa5..398f21d139 100644 --- a/ntcore/src/test/native/cpp/ValueTest.cpp +++ b/ntcore/src/test/native/cpp/ValueTest.cpp @@ -271,6 +271,8 @@ TEST_F(ValueTest, StringArray) { NT_DisposeValue(&cv); } +// Google Test doesn't have ASSERT_DEATH when compiled with emscripten +#ifndef __EMSCRIPTEN__ #ifdef NDEBUG TEST_F(ValueDeathTest, DISABLED_GetAssertions) { #else @@ -285,6 +287,7 @@ TEST_F(ValueDeathTest, GetAssertions) { ASSERT_DEATH((void)v.GetDoubleArray(), "type == NT_DOUBLE_ARRAY"); ASSERT_DEATH((void)v.GetStringArray(), "type == NT_STRING_ARRAY"); } +#endif TEST_F(ValueTest, UnassignedComparison) { Value v1, v2; diff --git a/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch b/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch index 114b51ae77..2822ebba58 100644 --- a/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch +++ b/upstream_utils/debugging_patches/0001-Guard-gnu-flatten-attribute.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 21 Nov 2024 17:51:15 -0800 -Subject: [PATCH 1/4] Guard [[gnu::flatten]] attribute +Subject: [PATCH 1/5] Guard [[gnu::flatten]] attribute --- include/debugging.hpp | 10 ++++++++-- diff --git a/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch b/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch index 94f5bd0f45..943ad80401 100644 --- a/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch +++ b/upstream_utils/debugging_patches/0002-Remove-debugger_query-argument-from-Windows-and-macO.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 21 Nov 2024 17:23:48 -0800 -Subject: [PATCH 2/4] Remove debugger_query argument from Windows and macOS +Subject: [PATCH 2/5] Remove debugger_query argument from Windows and macOS --- src/macos.cxx | 2 +- diff --git a/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch b/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch index d4e5b65970..49c47bf094 100644 --- a/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch +++ b/upstream_utils/debugging_patches/0003-Fix-exception-mask-type-typo-on-macOS.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 21 Nov 2024 18:09:37 -0800 -Subject: [PATCH 3/4] Fix exception mask type typo on macOS +Subject: [PATCH 3/5] Fix exception mask type typo on macOS --- src/macos.cxx | 2 +- diff --git a/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch b/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch index 0e4281e4ba..277d297c83 100644 --- a/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch +++ b/upstream_utils/debugging_patches/0004-Remove-NOMINMAX-macro-from-Windows.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 21 Nov 2024 18:49:53 -0800 -Subject: [PATCH 4/4] Remove NOMINMAX macro from Windows +Subject: [PATCH 4/5] Remove NOMINMAX macro from Windows --- src/windows.cxx | 1 - diff --git a/upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch b/upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch new file mode 100644 index 0000000000..eef5288a63 --- /dev/null +++ b/upstream_utils/debugging_patches/0005-Add-emscripten-shim.patch @@ -0,0 +1,34 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 8 Apr 2025 15:28:38 -0700 +Subject: [PATCH 5/5] Add emscripten shim + +--- + src/emscripten.cxx | 18 ++++++++++++++++++ + 1 file changed, 18 insertions(+) + create mode 100644 src/emscripten.cxx + +diff --git a/src/emscripten.cxx b/src/emscripten.cxx +new file mode 100644 +index 0000000000000000000000000000000000000000..16924894cc0c6085b27b33e6b9f2a6e6d582d116 +--- /dev/null ++++ b/src/emscripten.cxx +@@ -0,0 +1,18 @@ ++#if defined(__EMSCRIPTEN__) ++ ++# include ++ ++# include ++# include ++# include ++ ++namespace wpi { ++ ++bool is_debugger_present() noexcept ++{ ++ return false; ++} ++ ++} // namespace wpi ++ ++#endif diff --git a/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch b/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch index 8218faf96c..4bc812ce99 100644 --- a/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch +++ b/upstream_utils/memory_patches/0001-Group-doxygen-into-memory-module.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Wed, 21 Aug 2024 20:50:15 -0400 -Subject: [PATCH 1/2] Group doxygen into memory module +Subject: [PATCH 1/3] Group doxygen into memory module --- .../foonathan/memory/aligned_allocator.hpp | 2 +- diff --git a/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch b/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch index 08b58bf146..2afa05cc80 100644 --- a/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch +++ b/upstream_utils/memory_patches/0002-Remove-conflicting-doxygen-concept-alias.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Wed, 21 Aug 2024 21:47:32 -0400 -Subject: [PATCH 2/2] Remove conflicting doxygen 'concept' alias +Subject: [PATCH 2/3] Remove conflicting doxygen 'concept' alias \concept was added as a doxygen command in 1.9.2 and is meant to be applied to concepts. Inserting them into standard comment paragraphs causes doxygen to interpret the following text as a concept name and add it to the documentation, as well as remove the text from the paragraph. In the upstream repo, this alias links to markdown documentation, so it's not usable for us anyways. diff --git a/upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch b/upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch new file mode 100644 index 0000000000..c8bd3cd4a0 --- /dev/null +++ b/upstream_utils/memory_patches/0003-Fix-deprecation-warning-for-UDLs.patch @@ -0,0 +1,52 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 8 Apr 2025 15:30:06 -0700 +Subject: [PATCH 3/3] Fix deprecation warning for UDLs + +--- + include/foonathan/memory/memory_arena.hpp | 12 ++++++------ + 1 file changed, 6 insertions(+), 6 deletions(-) + +diff --git a/include/foonathan/memory/memory_arena.hpp b/include/foonathan/memory/memory_arena.hpp +index eb969a677329b5b2d536f39f9a15817f040cf79f..d91f2a58cef56278cdb091daf34cebba7ec5b92c 100644 +--- a/include/foonathan/memory/memory_arena.hpp ++++ b/include/foonathan/memory/memory_arena.hpp +@@ -656,32 +656,32 @@ namespace foonathan + /// \returns The number of bytes `value` is in the given unit. + /// \ingroup memory_core + /// @{ +- constexpr std::size_t operator"" _KiB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_KiB(unsigned long long value) noexcept + { + return std::size_t(value * 1024); + } + +- constexpr std::size_t operator"" _KB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_KB(unsigned long long value) noexcept + { + return std::size_t(value * 1000); + } + +- constexpr std::size_t operator"" _MiB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_MiB(unsigned long long value) noexcept + { + return std::size_t(value * 1024 * 1024); + } + +- constexpr std::size_t operator"" _MB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_MB(unsigned long long value) noexcept + { + return std::size_t(value * 1000 * 1000); + } + +- constexpr std::size_t operator"" _GiB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_GiB(unsigned long long value) noexcept + { + return std::size_t(value * 1024 * 1024 * 1024); + } + +- constexpr std::size_t operator"" _GB(unsigned long long value) noexcept ++ constexpr std::size_t operator""_GB(unsigned long long value) noexcept + { + return std::size_t(value * 1000 * 1000 * 1000); + } diff --git a/wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp b/wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp new file mode 100644 index 0000000000..16924894cc --- /dev/null +++ b/wpiutil/src/main/native/thirdparty/debugging/src/emscripten.cpp @@ -0,0 +1,18 @@ +#if defined(__EMSCRIPTEN__) + +# include + +# include +# include +# include + +namespace wpi { + +bool is_debugger_present() noexcept +{ + return false; +} + +} // namespace wpi + +#endif diff --git a/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp b/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp index dc6f195b67..43de18efe5 100644 --- a/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp +++ b/wpiutil/src/main/native/thirdparty/memory/include/wpi/memory/memory_arena.hpp @@ -656,32 +656,32 @@ namespace wpi /// \returns The number of bytes `value` is in the given unit. /// \ingroup memory_core /// @{ - constexpr std::size_t operator"" _KiB(unsigned long long value) noexcept + constexpr std::size_t operator""_KiB(unsigned long long value) noexcept { return std::size_t(value * 1024); } - constexpr std::size_t operator"" _KB(unsigned long long value) noexcept + constexpr std::size_t operator""_KB(unsigned long long value) noexcept { return std::size_t(value * 1000); } - constexpr std::size_t operator"" _MiB(unsigned long long value) noexcept + constexpr std::size_t operator""_MiB(unsigned long long value) noexcept { return std::size_t(value * 1024 * 1024); } - constexpr std::size_t operator"" _MB(unsigned long long value) noexcept + constexpr std::size_t operator""_MB(unsigned long long value) noexcept { return std::size_t(value * 1000 * 1000); } - constexpr std::size_t operator"" _GiB(unsigned long long value) noexcept + constexpr std::size_t operator""_GiB(unsigned long long value) noexcept { return std::size_t(value * 1024 * 1024 * 1024); } - constexpr std::size_t operator"" _GB(unsigned long long value) noexcept + constexpr std::size_t operator""_GB(unsigned long long value) noexcept { return std::size_t(value * 1000 * 1000 * 1000); } diff --git a/wpiutil/src/test/native/include/wpi/SpanMatcher.h b/wpiutil/src/test/native/include/wpi/SpanMatcher.h index 247c142956..8930c9acf0 100644 --- a/wpiutil/src/test/native/include/wpi/SpanMatcher.h +++ b/wpiutil/src/test/native/include/wpi/SpanMatcher.h @@ -70,6 +70,6 @@ void SpanMatcher::DescribeNegationTo(::std::ostream* os) const { } // namespace wpi -inline std::span operator"" _us(const char* str, size_t len) { +inline std::span operator""_us(const char* str, size_t len) { return {reinterpret_cast(str), len}; } From 3960045663dc647d8fb70fab3844e74ce54b51a6 Mon Sep 17 00:00:00 2001 From: me-it-is Date: Fri, 25 Apr 2025 21:58:05 -0700 Subject: [PATCH 33/34] [wpimath] TrapezoidProfile: Fix null pointer when calling timeLeftUntil (#7894) --- .../first/math/trajectory/TrapezoidProfile.java | 7 ++++--- .../include/frc/trajectory/TrapezoidProfile.h | 14 ++++++++------ .../math/trajectory/TrapezoidProfileTest.java | 7 +++++++ .../native/cpp/trajectory/TrapezoidProfileTest.cpp | 7 +++++++ 4 files changed, 26 insertions(+), 9 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index 448c8384cf..7e7bbdcff8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -43,7 +43,7 @@ public class TrapezoidProfile { private int m_direction; private final Constraints m_constraints; - private State m_current; + private State m_current = new State(); private double m_endAccel; private double m_endFullSpeed; @@ -187,7 +187,8 @@ public class TrapezoidProfile { * Returns the time left until a target distance in the profile is reached. * * @param target The target distance. - * @return The time left until a target distance in the profile is reached. + * @return The time left until a target distance in the profile is reached, or zero if no goal was + * set. */ public double timeLeftUntil(double target) { double position = m_current.position * m_direction; @@ -253,7 +254,7 @@ public class TrapezoidProfile { /** * Returns the total time the profile takes to reach the goal. * - * @return The total time the profile takes to reach the goal. + * @return The total time the profile takes to reach the goal, or zero if no goal was set. */ public double totalTime() { return m_endDecel; diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 9dd82f4eee..72224fda83 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -200,7 +200,8 @@ class TrapezoidProfile { * Returns the time left until a target distance in the profile is reached. * * @param target The target distance. - * @return The time left until a target distance in the profile is reached. + * @return The time left until a target distance in the profile is reached, or + * zero if no goal was set. */ constexpr units::second_t TimeLeftUntil(Distance_t target) const { Distance_t position = m_current.position * m_direction; @@ -271,7 +272,8 @@ class TrapezoidProfile { /** * Returns the total time the profile takes to reach the goal. * - * @return The total time the profile takes to reach the goal. + * @return The total time the profile takes to reach the goal, or zero if no + * goal was set. */ constexpr units::second_t TotalTime() const { return m_endDecel; } @@ -311,14 +313,14 @@ class TrapezoidProfile { } // The direction of the profile, either 1 for forwards or -1 for inverted - int m_direction; + int m_direction = 1; Constraints m_constraints; State m_current; - units::second_t m_endAccel; - units::second_t m_endFullSpeed; - units::second_t m_endDecel; + units::second_t m_endAccel = 0_s; + units::second_t m_endFullSpeed = 0_s; + units::second_t m_endDecel = 0_s; }; } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java index 64dfa4784d..72b730420f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java @@ -246,4 +246,11 @@ class TrapezoidProfileTest { } } } + + @Test + void initalizationOfCurrentState() { + var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1)); + assertNear(profile.timeLeftUntil(0), 0, 1e-10); + assertNear(profile.totalTime(), 0, 1e-10); + } } diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp index 39cbc3b0ea..e83da634b6 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) { } } } + +TEST(TrapezoidProfileTest, InitalizationOfCurrentState) { + frc::TrapezoidProfile::Constraints constraints{1_mps, 1_mps_sq}; + frc::TrapezoidProfile profile{constraints}; + EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s); + EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s); +} From 0695a4db8912a65f175c814c30075cc3e87b313f Mon Sep 17 00:00:00 2001 From: crueter Date: Sat, 26 Apr 2025 01:05:31 -0400 Subject: [PATCH 34/34] [wpimath] Fix Debouncer type-changing behavior (#7870) Closes #7867 Properly resets the baseline upon switching the debounce type, and adds a test for such. Signed-off-by: swurl --- .../main/java/edu/wpi/first/math/filter/Debouncer.java | 8 +++----- wpimath/src/main/native/cpp/filter/Debouncer.cpp | 10 +--------- wpimath/src/main/native/include/frc/filter/Debouncer.h | 6 +++++- .../java/edu/wpi/first/math/filter/DebouncerTest.java | 1 + wpimath/src/test/native/cpp/filter/DebouncerTest.cpp | 2 ++ 5 files changed, 12 insertions(+), 15 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java index a19de59f0b..28ac63033f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java @@ -40,11 +40,7 @@ public class Debouncer { resetTimer(); - m_baseline = - switch (m_debounceType) { - case kBoth, kRising -> false; - case kFalling -> true; - }; + m_baseline = m_debounceType == DebounceType.kFalling; } /** @@ -114,6 +110,8 @@ public class Debouncer { */ public void setDebounceType(DebounceType type) { m_debounceType = type; + + m_baseline = m_debounceType == DebounceType.kFalling; } /** diff --git a/wpimath/src/main/native/cpp/filter/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp index 841768eb20..18e23a0961 100644 --- a/wpimath/src/main/native/cpp/filter/Debouncer.cpp +++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp @@ -10,15 +10,7 @@ using namespace frc; Debouncer::Debouncer(units::second_t debounceTime, DebounceType type) : m_debounceTime(debounceTime), m_debounceType(type) { - switch (type) { - case DebounceType::kBoth: // fall-through - case DebounceType::kRising: - m_baseline = false; - break; - case DebounceType::kFalling: - m_baseline = true; - break; - } + m_baseline = m_debounceType == DebounceType::kFalling; ResetTimer(); } diff --git a/wpimath/src/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h index 45ed5df8e6..9b2d18b519 100644 --- a/wpimath/src/main/native/include/frc/filter/Debouncer.h +++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h @@ -71,7 +71,11 @@ class WPILIB_DLLEXPORT Debouncer { * * @param type Which type of state change the debouncing will be performed on. */ - constexpr void SetDebounceType(DebounceType type) { m_debounceType = type; } + constexpr void SetDebounceType(DebounceType type) { + m_debounceType = type; + + m_baseline = m_debounceType == DebounceType::kFalling; + } /** * Get the debounce type. diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java index 8cf8fd8812..726fda89d2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java @@ -84,5 +84,6 @@ class DebouncerTest { debouncer.setDebounceType(Debouncer.DebounceType.kFalling); assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling); + assertTrue(debouncer.calculate(false)); } } diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp index e1a990964b..8b0e56cd96 100644 --- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp +++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp @@ -72,4 +72,6 @@ TEST_F(DebouncerTest, DebounceParams) { EXPECT_TRUE(debouncer.GetDebounceType() == frc::Debouncer::DebounceType::kFalling); + + EXPECT_TRUE(debouncer.Calculate(false)); }