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 diff --git a/.github/workflows/bazel.yml b/.github/workflows/bazel.yml index 8949d6b055..af331b9679 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: diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index fc850f278d..770a194d14 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.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 4f8edc4fcb..83b5399e83 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.9 - uses: actions/checkout@v4 diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 9d989e411d..c70288f351 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 diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index 8640bcd5af..7c2b404fcf 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-18 libprotobuf-dev protobuf-compiler ninja-build - name: Install sccache - uses: mozilla-actions/sccache-action@v0.0.5 + uses: mozilla-actions/sccache-action@v0.0.9 - uses: actions/checkout@v4 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], 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 diff --git a/README.md b/README.md index 3576047006..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 @@ -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. 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/cscore/BUILD.bazel b/cscore/BUILD.bazel index 1c0ac8d9b7..a01afe2542 100644 --- a/cscore/BUILD.bazel +++ b/cscore/BUILD.bazel @@ -42,6 +42,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/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; + } } } diff --git a/datalog/doc/datalog.adoc b/datalog/doc/datalog.adoc index 9fd6f6f980..a6ea78c98e 100644 --- a/datalog/doc/datalog.adoc +++ b/datalog/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/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index 40d5d571dd..cb29c6d0ee 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())}, diff --git a/glass/src/libnt/native/cpp/NetworkTables.cpp b/glass/src/libnt/native/cpp/NetworkTables.cpp index e10ff93c09..db75f5db12 100644 --- a/glass/src/libnt/native/cpp/NetworkTables.cpp +++ b/glass/src/libnt/native/cpp/NetworkTables.cpp @@ -359,7 +359,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(); 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/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java index fc49a9cf5a..c2bc8b0f1a 100644 --- a/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java +++ b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java @@ -25,6 +25,7 @@ public class RomiMotor extends PWMMotorController { * @param channel The PWM channel that the RomiMotor is attached to. 0 is the left motor, 1 is the * right. */ + @SuppressWarnings("this-escape") public RomiMotor(final int channel) { super("Romi Motor", channel); initRomiMotor(); 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'" diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index ebb745d75f..43519b1545 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/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp index 952a630d0d..e1fde0e007 100644 --- a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp +++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp @@ -139,6 +139,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(" #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; 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/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 f1d4dbccad..ecab11f694 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 @@ -165,6 +183,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 -> @@ -201,6 +270,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' @@ -244,15 +348,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/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/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..5edd312bb9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -0,0 +1,23 @@ +[ + { + "name": "Encoder", + "description": "Snippets of Encoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder" + ], + "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/build.gradle b/wpilibjExamples/build.gradle index b480f486aa..0e8caf4f48 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' @@ -201,6 +203,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/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/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..abc1bd45e1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -0,0 +1,25 @@ +[ + { + "name": "Encoder", + "description": "Snippets of Encoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder" + ], + "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" + } +] 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/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 5bf89d7e81..1e374ec583 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() { @@ -44,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); } } @@ -209,4 +213,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/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 9966437f5b..f8ffb4be56 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 @@ -188,13 +188,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ */ public double calculate(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 f5c9df7e7a..36bab653c9 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 @@ -171,13 +171,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria */ public double calculate(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/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 03df366089..0d402c7af9 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, dt)); } + // 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/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java index 5aa38dafe4..7919f0607a 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_debounceTime; - private final DebounceType m_debounceType; + private double m_debounceTime; + private DebounceType m_debounceType; private boolean m_baseline; private double m_prevTime; @@ -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; } /** @@ -86,4 +82,44 @@ 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_debounceTime = 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_debounceTime; + } + + /** + * 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; + + m_baseline = m_debounceType == DebounceType.kFalling; + } + + /** + * 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/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index 4c0f8289c2..ad122f8475 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 @@ -42,7 +42,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; @@ -186,7 +186,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; @@ -252,7 +253,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/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/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/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(); diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index a0447ee2a1..060be0da91 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 { @@ -37,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); + } } /** @@ -207,4 +217,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/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 e12a76384e..ba1db8fde9 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -93,13 +93,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 * 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/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h index 9ab3e0e168..9b2d18b519 100644 --- a/wpimath/src/main/native/include/frc/filter/Debouncer.h +++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h @@ -48,6 +48,42 @@ 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; + + m_baseline = m_debounceType == DebounceType::kFalling; + } + + /** + * 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/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 1dad991c30..83e7d34e8f 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -198,7 +198,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; @@ -269,7 +270,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; } @@ -309,14 +311,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/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/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index eb336089e4..2ff1a0db86 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,46 +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.calculate(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.calculate(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.calculate(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/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index d9b645b9c0..aa723fcf52 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 @@ -60,17 +64,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 dt = 0.005; assertDoesNotThrow( () -> { @@ -78,8 +82,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), @@ -92,10 +96,10 @@ class UnscentedKalmanFilterTest { var u = VecBuilder.fill(12.0, 12.0); observer.predict(u, dt); - 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)); @@ -103,7 +107,7 @@ class UnscentedKalmanFilterTest { Nat.N5(), u, globalY, - UnscentedKalmanFilterTest::getGlobalMeasurementModel, + UnscentedKalmanFilterTest::driveGlobalMeasurementModel, R, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -113,16 +117,16 @@ class UnscentedKalmanFilterTest { } @Test - void testConvergence() { - double dt = 0.005; - double rb = 0.8382 / 2.0; // Robot radius + void testDriveConvergence() { + final double dt = 0.005; + final double rb = 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), @@ -146,7 +150,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())); @@ -161,7 +165,7 @@ class UnscentedKalmanFilterTest { var trueXhat = observer.getXhat(); double totalTime = trajectory.getTotalTime(); - for (int i = 0; i < (totalTime / dt); i++) { + for (int i = 0; i < (totalTime / dt); ++i) { var ref = trajectory.sample(dt * i); double vl = ref.velocity * (1 - (ref.curvature * rb)); double vr = ref.velocity * (1 + (ref.curvature * rb)); @@ -174,24 +178,26 @@ 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(dt); - 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, dt); r = nextR; - trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt); + trueXhat = + NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt); } - 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)); @@ -199,7 +205,7 @@ class UnscentedKalmanFilterTest { Nat.N5(), u, globalY, - UnscentedKalmanFilterTest::getGlobalMeasurementModel, + UnscentedKalmanFilterTest::driveGlobalMeasurementModel, R, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), @@ -236,7 +242,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))); @@ -264,4 +270,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 dt = 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, dt); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + for (int i = 0; i < steps; ++i) { + inputs.set(i, motorControlInput(i * dt)); + 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), + dt); + + 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), dt); + 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/java/edu/wpi/first/math/filter/DebouncerTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java index b6334de009..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 @@ -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,21 @@ 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); + assertTrue(debouncer.calculate(false)); + } } 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/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); +} 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); } 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 diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp index d232739b7a..8b0e56cd96 100644 --- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp +++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp @@ -56,3 +56,22 @@ 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); + + EXPECT_TRUE(debouncer.Calculate(false)); +} 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); +} 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. 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 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/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 } 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}; } diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java index 73901e4bfa..cbd8f61725 100644 --- a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java @@ -21,7 +21,7 @@ public class XRPServo { private static void checkDeviceAllocation(int deviceNum) { if (!s_simDeviceNameMap.containsKey(deviceNum)) { - throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5"); + throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-7"); } if (s_registeredDevices.contains(deviceNum)) { @@ -34,6 +34,8 @@ public class XRPServo { static { s_simDeviceNameMap.put(4, "servo1"); s_simDeviceNameMap.put(5, "servo2"); + s_simDeviceNameMap.put(6, "servo3"); + s_simDeviceNameMap.put(7, "servo4"); } private final SimDouble m_simPosition; diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp index 6df18b1cb8..5637ce4e2e 100644 --- a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp @@ -15,8 +15,8 @@ using namespace frc; -std::map 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 = {};