From d3b45bc76dc3e8a78761954c8cdea65d551a0c2d Mon Sep 17 00:00:00 2001 From: samfreund Date: Fri, 27 Mar 2026 12:19:36 -0500 Subject: [PATCH 01/15] upgrade pnpm-setup to node 24 --- .github/workflows/build.yml | 6 +++--- .github/workflows/lint-format.yml | 2 +- .github/workflows/photon-api-docs.yml | 2 +- .github/workflows/website.yml | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1f475a2060..72d0f42b73 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -110,7 +110,7 @@ jobs: java-version: 17 distribution: temurin - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node.js @@ -151,7 +151,7 @@ jobs: java-version: 17 distribution: temurin - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node.js @@ -353,7 +353,7 @@ jobs: java-version: 17 distribution: temurin - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node.js diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index db06c009fc..ee4b6ea32b 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -81,7 +81,7 @@ jobs: steps: - uses: actions/checkout@v6 - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node.js diff --git a/.github/workflows/photon-api-docs.yml b/.github/workflows/photon-api-docs.yml index 9a98af8694..5c651f13ac 100644 --- a/.github/workflows/photon-api-docs.yml +++ b/.github/workflows/photon-api-docs.yml @@ -31,7 +31,7 @@ jobs: steps: - uses: actions/checkout@v6 - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node.js diff --git a/.github/workflows/website.yml b/.github/workflows/website.yml index e91edab796..7d607e9fef 100644 --- a/.github/workflows/website.yml +++ b/.github/workflows/website.yml @@ -11,7 +11,7 @@ jobs: steps: - uses: actions/checkout@v6 - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node @@ -41,7 +41,7 @@ jobs: steps: - uses: actions/checkout@v6 - name: Install pnpm - uses: pnpm/action-setup@v4 + uses: pnpm/action-setup@v5 with: version: 10 - name: Setup Node From 70ed3cb7ed6ea824dabcd3f528aa8cd33b605ae4 Mon Sep 17 00:00:00 2001 From: samfreund Date: Fri, 27 Mar 2026 12:26:15 -0500 Subject: [PATCH 02/15] upgrade download-artifact to v7 --- .github/workflows/python.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index cc45cee79b..90682786d0 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -36,7 +36,7 @@ jobs: run: python setup.py sdist bdist_wheel - name: Upload artifacts - uses: actions/upload-artifact@v6 + uses: actions/upload-artifact@v7 with: name: dist path: ./photon-lib/py/dist/ @@ -62,7 +62,7 @@ jobs: pip install pytest mypy - name: Download artifacts - uses: actions/download-artifact@v6 + uses: actions/download-artifact@v7 with: name: dist path: dist/ @@ -102,7 +102,7 @@ jobs: python -m pip install --upgrade pip - name: Download artifacts - uses: actions/download-artifact@v6 + uses: actions/download-artifact@v7 with: name: dist path: ./photon-lib/py/dist/ From 40ef3db573e6ab84a0707646f1eba334b09dbf59 Mon Sep 17 00:00:00 2001 From: samfreund Date: Fri, 27 Mar 2026 22:57:16 -0500 Subject: [PATCH 03/15] upgrade upload artifact and configure archive param --- .github/workflows/build.yml | 54 +++++++++++++-------------- .github/workflows/lint-format.yml | 4 +- .github/workflows/photon-api-docs.yml | 8 ++-- .github/workflows/python.yml | 6 +-- 4 files changed, 35 insertions(+), 37 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 72d0f42b73..efffdaa78f 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -58,10 +58,10 @@ jobs: du -h . | sort -h if: matrix.os == 'ubuntu-24.04' # Download prebuilt photonlib artifacts - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: maven-${{ matrix.artifact-name }} - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: maven-Athena - name: Move to maven local @@ -127,10 +127,10 @@ jobs: - name: Run Playwright tests working-directory: photon-client run: pnpm test - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 if: ${{ !cancelled() }} with: - name: playwright-report + archive: false path: photon-client/playwright-report/ retention-days: 30 build-gradle: @@ -184,7 +184,7 @@ jobs: working-directory: docs run: | make html - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: built-docs path: docs/build/html @@ -214,9 +214,9 @@ jobs: mv photon-lib/build/generated/vendordeps/photonlib.json photon-lib/build/generated/vendordeps/photonlib-$(git describe --tags --match=v*).json # Upload it here so it shows up in releases - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: - name: photonlib-vendor-json + archive: false path: photon-lib/build/generated/vendordeps/photonlib-*.json build-photonlib-host: @@ -255,7 +255,7 @@ jobs: if: github.event_name == 'push' && github.repository_owner == 'photonvision' # Copy artifacts to build/outputs/maven - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: maven-${{ matrix.artifact-name }} path: build/outputs @@ -296,7 +296,7 @@ jobs: if: github.event_name == 'push' && github.repository_owner == 'photonvision' # Copy artifacts to build/outputs/maven - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts ${{ matrix.build-options }} - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: maven-${{ matrix.artifact-name }} path: build/outputs @@ -311,7 +311,7 @@ jobs: fetch-depth: 0 - run: git fetch --tags --force # download all maven-* artifacts to outputs/ - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: merge-multiple: true path: output @@ -321,7 +321,7 @@ jobs: name: ZIP stuff up working-directory: output - run: ls output - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: photonlib-offline path: output/*.zip @@ -365,7 +365,7 @@ jobs: - name: Install Arm64 Toolchain run: ./gradlew installArm64Toolchain if: ${{ (matrix.artifact-name) == 'LinuxArm64' }} - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: built-docs path: photon-server/src/main/resources/web/docs @@ -373,11 +373,11 @@ jobs: if: ${{ (matrix.arch-override != 'none') }} - run: ./gradlew photon-server:shadowJar if: ${{ (matrix.arch-override == 'none') }} - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: - name: jar-${{ matrix.artifact-name }} + archive: false path: photon-server/build/libs - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: photon-targeting_jar-${{ matrix.artifact-name }} path: photon-targeting/build/libs @@ -440,7 +440,7 @@ jobs: with: java-version: 17 distribution: temurin - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: ${{ matrix.artifact-name }} # The jar is run twice to exercise different code paths. @@ -573,7 +573,7 @@ jobs: uses: actions/checkout@v6 with: fetch-depth: 0 - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: jar-${{ matrix.artifact-name }} - uses: photonvision/photon-image-runner@HEAD @@ -611,10 +611,9 @@ jobs: # Point smoketest to the old image echo "smoketest_image_loc=${{ steps.generate_image.outputs.image }}" >> $GITHUB_ENV - - uses: actions/upload-artifact@v6 - name: Upload image + - uses: actions/upload-artifact@v7 with: - name: image-${{ matrix.image_suffix }} + archive: false path: photonvision*.xz # This is done after uploading the image to avoid contaminating the image with logs, caches, etc. @@ -643,25 +642,24 @@ jobs: runs-on: ubuntu-24.04 steps: # Download all fat JARs - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: merge-multiple: true - pattern: jar-* + pattern: photonvision-*.jar # Download offline photonlib - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: merge-multiple: true pattern: photonlib-offline # Download vendor json - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: - merge-multiple: true - pattern: photonlib-vendor-json + pattern: photonlib-*.json # Download all images - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: merge-multiple: true - pattern: image-* + pattern: photonvision-*.xz - run: find # Push to dev release diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index ee4b6ea32b..7477309a5a 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -46,9 +46,9 @@ jobs: - name: Generate diff run: git diff HEAD > wpiformat-fixes.patch if: ${{ failure() }} - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: - name: wpiformat fixes + archive: false path: wpiformat-fixes.patch if: ${{ failure() }} javaformat: diff --git a/.github/workflows/photon-api-docs.yml b/.github/workflows/photon-api-docs.yml index 5c651f13ac..071fd354a4 100644 --- a/.github/workflows/photon-api-docs.yml +++ b/.github/workflows/photon-api-docs.yml @@ -44,7 +44,7 @@ jobs: run: pnpm i --frozen-lockfile - name: Build Production Client run: pnpm run build-demo - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: built-demo path: photon-client/dist/ @@ -69,7 +69,7 @@ jobs: run: | chmod +x gradlew ./gradlew photon-docs:generateJavaDocs photon-docs:doxygen - - uses: actions/upload-artifact@v6 + - uses: actions/upload-artifact@v7 with: name: docs-java-cpp path: photon-docs/build/docs @@ -80,7 +80,7 @@ jobs: runs-on: ubuntu-24.04 steps: # Download docs artifact - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: pattern: docs-* - run: find . @@ -106,7 +106,7 @@ jobs: needs: [build_demo] runs-on: ubuntu-24.04 steps: - - uses: actions/download-artifact@v7 + - uses: actions/download-artifact@v8 with: name: built-demo - run: find . diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 90682786d0..4aa136791f 100644 --- a/.github/workflows/python.yml +++ b/.github/workflows/python.yml @@ -62,7 +62,7 @@ jobs: pip install pytest mypy - name: Download artifacts - uses: actions/download-artifact@v7 + uses: actions/download-artifact@v8 with: name: dist path: dist/ @@ -102,7 +102,7 @@ jobs: python -m pip install --upgrade pip - name: Download artifacts - uses: actions/download-artifact@v7 + uses: actions/download-artifact@v8 with: name: dist path: ./photon-lib/py/dist/ @@ -131,7 +131,7 @@ jobs: steps: - name: Download artifacts - uses: actions/download-artifact@v6 + uses: actions/download-artifact@v8 with: name: dist path: dist/ From 1024c03a94d7debb951973acc576dca013e976a2 Mon Sep 17 00:00:00 2001 From: samfreund Date: Sat, 28 Mar 2026 01:39:47 -0500 Subject: [PATCH 04/15] remove artifact name --- .github/workflows/build.yml | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index efffdaa78f..1a7f53c675 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -479,85 +479,71 @@ jobs: matrix: include: - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: RaspberryPi plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_raspi.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: limelight2 plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_limelight.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: limelight3 plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_limelight3.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: limelight3G plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_limelight3g.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: limelight4 plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_limelight4.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: luma_p1 plat_override: LINUX_RASPBIAN64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_luma_p1.img.xz minimum_free_mb: 100 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi5 plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi5.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi5b plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi5b.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi5plus plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi5plus.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi5pro plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi5pro.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi5max plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi5max.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: rock5c plat_override: LINUX_RK3588_64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_rock5c.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: orangepi6plus plat_override: LINUX_AARCH64 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_opi6plus.img.xz minimum_free_mb: 1024 - os: ubuntu-24.04-arm - artifact-name: LinuxArm64 image_suffix: rubikpi3 plat_override: LINUX_QCS6490 image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/$IMAGE_VERSION/photonvision_rubikpi3.tar.xz @@ -575,7 +561,7 @@ jobs: fetch-depth: 0 - uses: actions/download-artifact@v8 with: - name: jar-${{ matrix.artifact-name }} + name: photonvision-*-linuxarm64.jar - uses: photonvision/photon-image-runner@HEAD name: Generate image id: generate_image From d052875fdaf48a440a17fa9b6360bf5155f26dd0 Mon Sep 17 00:00:00 2001 From: samfreund Date: Sat, 28 Mar 2026 01:45:51 -0500 Subject: [PATCH 05/15] update refs --- .github/workflows/build.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1a7f53c675..dd3d907f03 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -425,12 +425,13 @@ jobs: matrix: include: - os: ubuntu-24.04 - artifact-name: jar-Linux + artifact-name: photonvision-*-linuxx64.jar extraOpts: -Djdk.lang.Process.launchMechanism=vfork - os: windows-latest - artifact-name: jar-Win64 + artifact-name: photonvision-*-winx64.jar - os: macos-latest - artifact-name: jar-macOS + artifact-name: photonvision-*-macarm64.jar + runs-on: ${{ matrix.os }} @@ -442,7 +443,7 @@ jobs: distribution: temurin - uses: actions/download-artifact@v8 with: - name: ${{ matrix.artifact-name }} + pattern: ${{ matrix.artifact-name }} # The jar is run twice to exercise different code paths. - run: | echo "=== First run ===" @@ -561,7 +562,7 @@ jobs: fetch-depth: 0 - uses: actions/download-artifact@v8 with: - name: photonvision-*-linuxarm64.jar + pattern: photonvision-*-linuxarm64.jar - uses: photonvision/photon-image-runner@HEAD name: Generate image id: generate_image From 64995775b86f13cd9ddd2dee6335a67ee7f2bfa1 Mon Sep 17 00:00:00 2001 From: samfreund Date: Fri, 3 Apr 2026 13:12:46 -0500 Subject: [PATCH 06/15] bah ci From bf4f818839a3a0777c6c5e9de9d67167ec6fe83d Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Thu, 19 Jun 2025 18:48:55 -0400 Subject: [PATCH 07/15] Switch to Ubuntu 24.04 --- .github/workflows/build.yml | 7 ++----- docs/source/docs/hardware/selecting-hardware.md | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1f475a2060..647e52b4ee 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -265,13 +265,10 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2025-24.04 - artifact-name: Athena - build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 artifact-name: Raspbian build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-24.04 artifact-name: Aarch64 build-options: "-Ponlylinuxarm64" diff --git a/docs/source/docs/hardware/selecting-hardware.md b/docs/source/docs/hardware/selecting-hardware.md index b9baa4d7b9..86b4040113 100644 --- a/docs/source/docs/hardware/selecting-hardware.md +++ b/docs/source/docs/hardware/selecting-hardware.md @@ -10,7 +10,7 @@ In order to use PhotonVision, you need a coprocessor and a camera. This page dis ### Minimum System Requirements -- Ubuntu 22.04 LTS or Windows 10/11 +- Ubuntu 24.04 LTS or Windows 10/11 - We don't recommend using Windows for anything except testing out the system on a local machine. - CPU: ARM Cortex-A53 (the CPU on Raspberry Pi 3) or better - At least 8GB of storage From 30348b1e90c3243910822206bfab120d8a4bb84d Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 17 Jun 2025 19:09:09 -0400 Subject: [PATCH 08/15] Upgrade to 2027 alpha --- .github/workflows/build.yml | 10 +++--- README.md | 2 +- build.gradle | 9 +++--- photon-core/build.gradle | 1 + .../networktables/NetworkTablesManager.java | 2 +- photon-lib/build.gradle | 2 ++ .../java/org/photonvision/PhotonCamera.java | 3 +- .../org/photonvision/PhotonPoseEstimator.java | 3 +- .../main/native/cpp/photon/PhotonCamera.cpp | 2 +- .../native/cpp/photon/PhotonPoseEstimator.cpp | 3 +- .../org/photonvision/PhotonCameraTest.java | 4 +-- photon-targeting/build.gradle | 4 +++ .../main/native/jni/FileLoggerExtrasJNI.cpp | 4 +-- .../aimandrange/build.gradle | 25 +++++---------- .../aimandrange/settings.gradle | 10 +++--- .../aimattarget/build.gradle | 25 +++++---------- .../aimattarget/settings.gradle | 10 +++--- photonlib-cpp-examples/poseest/build.gradle | 25 +++++---------- .../poseest/settings.gradle | 10 +++--- .../aimandrange/build.gradle | 32 +++++++++---------- .../aimandrange/settings.gradle | 9 ++++-- .../aimattarget/build.gradle | 32 +++++++++---------- .../aimattarget/settings.gradle | 9 ++++-- photonlib-java-examples/poseest/build.gradle | 32 +++++++++---------- .../poseest/settings.gradle | 9 ++++-- shared/common.gradle | 1 + shared/config.gradle | 1 - shared/javacommon.gradle | 1 + 28 files changed, 137 insertions(+), 143 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 647e52b4ee..1597b74cfe 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -50,11 +50,11 @@ jobs: with: java-version: 17 distribution: temurin - - name: Install RoboRIO Toolchain - run: ./gradlew installRoboRioToolchain + - name: Install SystemCore Toolchain + run: ./gradlew installSystemCoreToolchain - name: Delete duplicate toolchains run: | - find ~/.gradle/cache/ -name *roborio-academic* -exec rm -rf {} + + find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} + du -h . | sort -h if: matrix.os == 'ubuntu-24.04' # Download prebuilt photonlib artifacts @@ -252,7 +252,7 @@ jobs: name: Publish env: ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} - if: github.event_name == 'push' && github.repository_owner == 'photonvision' + if: github.event_name == 'push' && github.repository_owner == 'photonvision' && github.ref == 'refs/heads/main' # Copy artifacts to build/outputs/maven - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts - uses: actions/upload-artifact@v6 @@ -290,7 +290,7 @@ jobs: run: ./gradlew photon-lib:publish photon-targeting:publish ${{ matrix.build-options }} env: ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} - if: github.event_name == 'push' && github.repository_owner == 'photonvision' + if: github.event_name == 'push' && github.repository_owner == 'photonvision' && github.ref == 'refs/heads/main' # Copy artifacts to build/outputs/maven - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts ${{ matrix.build-options }} - uses: actions/upload-artifact@v6 diff --git a/README.md b/README.md index fc8400e25d..26515a19cc 100644 --- a/README.md +++ b/README.md @@ -45,7 +45,7 @@ Note that these are case sensitive! - `-Pprofile`: enables JVM profiling - `-PwithSanitizers`: On Linux, enables `-fsanitize=address,undefined,leak` -If you're cross-compiling, you'll need the WPILib toolchain installed. This must be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installRoboRioToolchain` +If you're cross-compiling, you'll need the WPILib toolchain installed. This must be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installSystemCoreToolchain` ## Out-of-Source Dependencies diff --git a/build.gradle b/build.gradle index 074dad7d65..232f450829 100644 --- a/build.gradle +++ b/build.gradle @@ -3,8 +3,8 @@ import edu.wpi.first.toolchain.* plugins { id "cpp" id "com.diffplug.spotless" version "8.1.0" - id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2025.0" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" id 'org.photonvision.tools.WpilibTools' version '2.4.1-photon' id 'com.google.protobuf' version '0.9.3' apply false id 'edu.wpi.first.GradleJni' version '1.1.0' @@ -21,6 +21,7 @@ allprojects { maven { url = "https://maven.photonvision.org/releases" } maven { url = "https://maven.photonvision.org/snapshots" } } + wpilibRepositories.use2027Repos() wpilibRepositories.addAllReleaseRepositories(it) wpilibRepositories.addAllDevelopmentRepositories(it) } @@ -32,7 +33,7 @@ ext.allOutputsFolder = file("$project.buildDir/outputs") apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2026.2.1" + wpilibVersion = "2027.0.0-alpha-1" wpimathVersion = wpilibVersion openCVYear = "2025" openCVversion = "4.10.0-3" @@ -40,7 +41,7 @@ ext { libcameraDriverVersion = "v2026.0.0" rknnVersion = "dev-v2026.0.1-1-g89b2888" rubikVersion = "dev-v2026.0.1-4-g13d6279" - frcYear = "2026" + frcYear = "2027_alpha1" mrcalVersion = "dev-v2026.0.0-1-g239d80e"; pubVersion = versionString diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 4ef927f6ce..aa66fc9671 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -23,6 +23,7 @@ dependencies { wpilibNatives wpilibTools.deps.wpilib("wpimath") wpilibNatives wpilibTools.deps.wpilib("wpinet") wpilibNatives wpilibTools.deps.wpilib("wpiutil") + wpilibNatives wpilibTools.deps.wpilib("datalog") wpilibNatives wpilibTools.deps.wpilib("ntcore") wpilibNatives wpilibTools.deps.wpilib("cscore") wpilibNatives wpilibTools.deps.wpilib("apriltag") diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index e713edcbb2..bad248e26d 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -358,7 +358,7 @@ private void setClientMode(NetworkConfig config) { ntInstance.stopClient(); String hostname = config.shouldManage ? config.hostname : CameraServerJNI.getHostname(); logger.debug("Starting NT Client with hostname: " + hostname); - ntInstance.startClient4(hostname); + ntInstance.startClient(hostname); try { int t = Integer.parseInt(config.ntServerAddress); if (!m_isRetryingConnection) logger.info("Starting NT Client, server team is " + t); diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 3645931ba5..5703c9081d 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -90,6 +90,7 @@ model { } } + nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "cscore_shared") nativeUtils.useRequiredLibrary(it, "cameraserver_shared") nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") @@ -369,6 +370,7 @@ dependencies { nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("datalog") nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index d548e17f09..0d4cb7f179 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,7 +24,6 @@ package org.photonvision; -import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; @@ -176,8 +175,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { new MultiSubscriber( instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); - HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); InstanceCount++; + HAL.reportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); // HACK - start a TimeSyncServer, if we haven't yet. TimeSyncSingleton.load(); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 2b560abe75..f7f0c2c1dc 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -26,7 +26,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; @@ -184,8 +183,8 @@ public PhotonPoseEstimator( OpenCvLoader.forceStaticLoad(); } - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); InstanceCount++; + HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); } /** Invalidates the pose cache. */ diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 1c4833c4bf..bb00064788 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -166,8 +166,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, frc::Alert::AlertType::kWarning), timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) { verifyDependencies(); - HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); InstanceCount++; + HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); // The Robot class is actually created here: // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33 diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index b49a462192..a720898093 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -86,9 +86,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, referencePose(frc::Pose3d()), poseCacheTimestamp(-1_s), headingBuffer(frc::TimeInterpolatableBuffer(1_s)) { - HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, - InstanceCount); InstanceCount++; + HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); } void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 9a2f69dd42..c5ed3189b4 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -253,14 +253,14 @@ public void testRestartingRobotAndCoproc( if (i == coprocStart || i == coprocRestart) { coprocNt.setServer("127.0.0.1", 5940); - coprocNt.startClient4("testClient"); + coprocNt.startClient("testClient"); // PhotonCamera makes a server by default - connect to it tspClient = new TimeSyncClient("127.0.0.1", 5810, 0.5); } if (i == robotStart || i == robotRestart) { - robotNt.startServer("networktables_random.json", "", 5941, 5940); + robotNt.startServer("networktables_random.json", "", 5940); } Thread.sleep(100); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 0ee500035d..7bf667bcc9 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -55,6 +55,7 @@ model { } nativeUtils.useRequiredLibrary(it, "wpiutil_shared") + nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "wpimath_shared") nativeUtils.useRequiredLibrary(it, "wpinet_shared") nativeUtils.useRequiredLibrary(it, "ntcore_shared") @@ -83,6 +84,7 @@ model { } nativeUtils.useRequiredLibrary(it, "wpiutil_shared") + nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "wpimath_shared") nativeUtils.useRequiredLibrary(it, "wpinet_shared") nativeUtils.useRequiredLibrary(it, "ntcore_shared") @@ -120,6 +122,7 @@ model { nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") + nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "apriltag_shared") nativeUtils.useRequiredLibrary(it, "cscore_shared") nativeUtils.useRequiredLibrary(it, "opencv_shared") @@ -179,6 +182,7 @@ def nativeTasks = wpilibTools.createExtractionTasks { nativeTasks.addToSourceSetResources(sourceSets.test) nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("datalog") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") diff --git a/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp b/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp index f65d089777..f0f36c3de9 100644 --- a/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp +++ b/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include "jni_utils.h" #include "org_photonvision_jni_QueuedFileLogger.h" @@ -30,7 +30,7 @@ struct QueuedFileLogger { std::mutex m_mutex; - wpi::FileLogger logger; + wpi::log::FileLogger logger; explicit QueuedFileLogger(std::string_view file) : logger{file, std::bind(&QueuedFileLogger::callback, this, diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index e18b1d8785..4c6930e3b7 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } repositories { @@ -11,14 +11,13 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you @@ -36,8 +35,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on SystemCore that no // longer exist in deploy directory of this project } } @@ -45,7 +44,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp +def deployArtifact = deploy.targets.systemcore.artifacts.frcCpp // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -61,15 +60,7 @@ wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { - // We don't need to build for roborio -- if we do, we need to install - // a roborio toolchain every time we build in CI - // Ideally, we'd be able to set the roborio toolchain as optional, but - // I can't figure out how to set that environment variable from build.gradle - // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) - // for now, commented out - - // targetPlatform wpi.platforms.roborio - + targetPlatform wpi.platforms.systemcore if (includeDesktopSupport) { targetPlatform wpi.platforms.desktop } diff --git a/photonlib-cpp-examples/aimandrange/settings.gradle b/photonlib-cpp-examples/aimandrange/settings.gradle index 882b61321a..64213ce87a 100644 --- a/photonlib-cpp-examples/aimandrange/settings.gradle +++ b/photonlib-cpp-examples/aimandrange/settings.gradle @@ -3,9 +3,8 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { mavenLocal() - jcenter() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -21,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index e18b1d8785..4c6930e3b7 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } repositories { @@ -11,14 +11,13 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you @@ -36,8 +35,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on SystemCore that no // longer exist in deploy directory of this project } } @@ -45,7 +44,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp +def deployArtifact = deploy.targets.systemcore.artifacts.frcCpp // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -61,15 +60,7 @@ wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { - // We don't need to build for roborio -- if we do, we need to install - // a roborio toolchain every time we build in CI - // Ideally, we'd be able to set the roborio toolchain as optional, but - // I can't figure out how to set that environment variable from build.gradle - // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) - // for now, commented out - - // targetPlatform wpi.platforms.roborio - + targetPlatform wpi.platforms.systemcore if (includeDesktopSupport) { targetPlatform wpi.platforms.desktop } diff --git a/photonlib-cpp-examples/aimattarget/settings.gradle b/photonlib-cpp-examples/aimattarget/settings.gradle index 882b61321a..64213ce87a 100644 --- a/photonlib-cpp-examples/aimattarget/settings.gradle +++ b/photonlib-cpp-examples/aimattarget/settings.gradle @@ -3,9 +3,8 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { mavenLocal() - jcenter() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -21,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/photonlib-cpp-examples/poseest/build.gradle b/photonlib-cpp-examples/poseest/build.gradle index e18b1d8785..4c6930e3b7 100644 --- a/photonlib-cpp-examples/poseest/build.gradle +++ b/photonlib-cpp-examples/poseest/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } repositories { @@ -11,14 +11,13 @@ repositories { wpi.maven.useLocal = false wpi.maven.useDevelopment = false -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you @@ -36,8 +35,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on SystemCore that no // longer exist in deploy directory of this project } } @@ -45,7 +44,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp +def deployArtifact = deploy.targets.systemcore.artifacts.frcCpp // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -61,15 +60,7 @@ wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { - // We don't need to build for roborio -- if we do, we need to install - // a roborio toolchain every time we build in CI - // Ideally, we'd be able to set the roborio toolchain as optional, but - // I can't figure out how to set that environment variable from build.gradle - // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) - // for now, commented out - - // targetPlatform wpi.platforms.roborio - + targetPlatform wpi.platforms.systemcore if (includeDesktopSupport) { targetPlatform wpi.platforms.desktop } diff --git a/photonlib-cpp-examples/poseest/settings.gradle b/photonlib-cpp-examples/poseest/settings.gradle index 882b61321a..64213ce87a 100644 --- a/photonlib-cpp-examples/poseest/settings.gradle +++ b/photonlib-cpp-examples/poseest/settings.gradle @@ -3,9 +3,8 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { mavenLocal() - jcenter() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -21,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 1ddca97530..4d0a2bd626 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,10 +1,12 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" @@ -13,20 +15,18 @@ repositories { } wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" - +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(4512) + team = project.frc.getTeamOrDefault(5940) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -39,8 +39,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on systemcore that no // longer exist in deploy directory of this project } } @@ -48,7 +48,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcJava +def deployArtifact = deploy.targets.systemcore.artifacts.frcJava // Set to true to use debug for all targets including JNI, which will drastically impact // performance. @@ -64,11 +64,11 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore) + systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore) - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore) + systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore) nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) diff --git a/photonlib-java-examples/aimandrange/settings.gradle b/photonlib-java-examples/aimandrange/settings.gradle index 699b164965..64213ce87a 100644 --- a/photonlib-java-examples/aimandrange/settings.gradle +++ b/photonlib-java-examples/aimandrange/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index b83b3a8bc5..f4401d5af8 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,28 +1,28 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" - +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(1736) + team = project.frc.getTeamOrDefault(5940) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -35,8 +35,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on systemcore that no // longer exist in deploy directory of this project } } @@ -44,7 +44,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcJava +def deployArtifact = deploy.targets.systemcore.artifacts.frcJava // Set to true to use debug for all targets including JNI, which will drastically impact // performance. @@ -60,11 +60,11 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore) + systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore) - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore) + systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore) nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) diff --git a/photonlib-java-examples/aimattarget/settings.gradle b/photonlib-java-examples/aimattarget/settings.gradle index 699b164965..64213ce87a 100644 --- a/photonlib-java-examples/aimattarget/settings.gradle +++ b/photonlib-java-examples/aimattarget/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index af021b3e32..f4401d5af8 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -1,28 +1,28 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" } -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = "2026.2.1" -wpi.versions.wpimathVersion = "2026.2.1" - +wpi.versions.wpilibVersion = "2027.0.+" -// Define my targets (RoboRIO) and artifacts (deployable files) +// Define my targets (SystemCore) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roborio(getTargetTypeClass('RoboRIO')) { + systemcore(getTargetTypeClass('SystemCore')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(4512) + team = project.frc.getTeamOrDefault(5940) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -35,8 +35,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on systemcore that no // longer exist in deploy directory of this project } } @@ -44,7 +44,7 @@ deploy { } } -def deployArtifact = deploy.targets.roborio.artifacts.frcJava +def deployArtifact = deploy.targets.systemcore.artifacts.frcJava // Set to true to use debug for all targets including JNI, which will drastically impact // performance. @@ -60,11 +60,11 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore) + systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore) - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore) + systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore) nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) diff --git a/photonlib-java-examples/poseest/settings.gradle b/photonlib-java-examples/poseest/settings.gradle index 699b164965..64213ce87a 100644 --- a/photonlib-java-examples/poseest/settings.gradle +++ b/photonlib-java-examples/poseest/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2026' + String frcYear = '2027_alpha1' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,11 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/shared/common.gradle b/shared/common.gradle index a1dd8e3bf7..c13a53ef4f 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -24,6 +24,7 @@ dependencies { implementation 'org.msgpack:jackson-dataformat-msgpack:0.9.0' implementation wpilibTools.deps.wpilibJava("wpiutil") + implementation wpilibTools.deps.wpilibJava("datalog") implementation wpilibTools.deps.wpilibJava("cameraserver") implementation wpilibTools.deps.wpilibJava("cscore") implementation wpilibTools.deps.wpilibJava("wpinet") diff --git a/shared/config.gradle b/shared/config.gradle index 07c01bd490..fbb48de0af 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -1,6 +1,5 @@ // Configure Native-Utils WPI Plugin nativeUtils.addWpiNativeUtils() -nativeUtils.withCrossRoboRIO() nativeUtils.withCrossLinuxArm32() nativeUtils.withCrossLinuxArm64() diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 1b99ace63a..9431f5f1fe 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -134,6 +134,7 @@ dependencies { } implementation wpilibTools.deps.wpilibJava("wpiutil") + implementation wpilibTools.deps.wpilibJava("datalog") implementation wpilibTools.deps.wpilibJava("cameraserver") implementation wpilibTools.deps.wpilibJava("cscore") implementation wpilibTools.deps.wpilibJava("wpinet") From 373a0eda4aad1f207ec4b1b69f969cfe2632f8bf Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Tue, 24 Jun 2025 10:17:44 +0800 Subject: [PATCH 09/15] Start on updating examples for 2027 Signed-off-by: Jade Turner --- .gitignore | 2 + .../src/main/cpp/subsystems/SwerveDrive.cpp | 10 ++--- .../src/main/include/subsystems/SwerveDrive.h | 9 ++-- .../aimandrange/build.gradle | 2 +- .../subsystems/drivetrain/SwerveDrive.java | 42 +++++++++---------- .../subsystems/drivetrain/SwerveDriveSim.java | 6 +-- .../subsystems/drivetrain/SwerveModule.java | 8 ++-- .../aimattarget/build.gradle | 2 +- .../subsystems/drivetrain/SwerveDrive.java | 42 +++++++++---------- .../subsystems/drivetrain/SwerveDriveSim.java | 6 +-- .../subsystems/drivetrain/SwerveModule.java | 8 ++-- photonlib-java-examples/poseest/build.gradle | 2 +- .../robot/subsystems/GamepieceLauncher.java | 4 +- .../subsystems/drivetrain/SwerveDrive.java | 42 +++++++++---------- .../subsystems/drivetrain/SwerveDriveSim.java | 6 +-- .../subsystems/drivetrain/SwerveModule.java | 8 ++-- 16 files changed, 100 insertions(+), 99 deletions(-) diff --git a/.gitignore b/.gitignore index ca51fb8e50..032a1d7e11 100644 --- a/.gitignore +++ b/.gitignore @@ -161,3 +161,5 @@ photon-client/playwright/.auth/ shell.nix flake.nix flake.lock + +/workspace diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp index 4602def8a9..40ba22f6b4 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp @@ -32,7 +32,7 @@ SwerveDrive::SwerveDrive() : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), - gyroSim(gyro), + // gyroSim(gyro), swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), constants::Swerve::kDriveGearRatio, constants::Swerve::kWheelDiameter / 2, @@ -82,8 +82,8 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { for (int i = 0; i < swerveMods.size(); i++) { swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); } - gyroSim.SetAngle(-pose.Rotation().Degrees()); - gyroSim.SetRate(0_rad_per_s); + // gyroSim.SetAngle(-pose.Rotation().Degrees()); + // gyroSim.SetRate(0_rad_per_s); } poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); @@ -190,8 +190,8 @@ void SwerveDrive::SimulationPeriodic() { swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.SetRate(-swerveDriveSim.GetOmega()); - gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); + // gyroSim.SetRate(-swerveDriveSim.GetOmega()); + // gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); } frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h index 98b0b2a582..f47638fbdc 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,11 +24,9 @@ #pragma once -#include -#include #include #include -#include +#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -70,11 +68,12 @@ class SwerveDrive { swerveMods[2].GetModuleConstants().centerOffset, swerveMods[3].GetModuleConstants().centerOffset, }; - frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::OnboardIMU gyro{MountOrientation::kFlat}; frc::SwerveDrivePoseEstimator<4> poseEstimator; frc::ChassisSpeeds targetChassisSpeeds{}; - frc::sim::ADXRS450_GyroSim gyroSim; + //TODO(Jade) onboard imu doesn't have sim yet + // frc::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 4d0a2bd626..131232caad 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -78,7 +78,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index c1676f6ff4..871414f871 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) Onboard IMU doesnt have sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public SwerveDrive() { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public void periodic() { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public void resetPose(Pose2d pose, boolean resetSimPose) { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public void log() { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public void simulationPeriodic() { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6c..07bff21152 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public void reset( driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public double getOmegaRadsPerSec() { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 5d77dcce51..6242a4e7ca 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -88,12 +88,12 @@ public void periodic() { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public void log() { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index f4401d5af8..4622163ee8 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -74,7 +74,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index c1676f6ff4..7feea5c446 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) OnboardIMU doesn't have sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public SwerveDrive() { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public void periodic() { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public void resetPose(Pose2d pose, boolean resetSimPose) { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public void log() { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public void simulationPeriodic() { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6c..07bff21152 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public void reset( driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public double getOmegaRadsPerSec() { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 5d77dcce51..6242a4e7ca 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -88,12 +88,12 @@ public void periodic() { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public void log() { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index f4401d5af8..4622163ee8 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -74,7 +74,7 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.12.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java index 8d76b1d788..cb1a6ecb01 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -51,7 +51,7 @@ public void setRunning(boolean shouldRun) { public void periodic() { double maxRPM = - Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec); + Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); motor.set(curMotorCmd); @@ -76,7 +76,7 @@ private void simulationInit() { public void simulationPeriodic() { launcherSim.setInputVoltage(curMotorCmd * RobotController.getBatteryVoltage()); launcherSim.update(0.02); - var spd = launcherSim.getAngularVelocityRPM(); + var spd = launcherSim.getAngularVelocity(); SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd); } } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index c1676f6ff4..60e1258029 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -38,9 +38,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.*; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.OnboardIMU; +import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; @@ -62,7 +61,7 @@ public class SwerveDrive { swerveMods[2].getModuleConstants().centerOffset, swerveMods[3].getModuleConstants().centerOffset); - private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + private final OnboardIMU gyro = new OnboardIMU(MountOrientation.kFlat); // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; @@ -70,7 +69,8 @@ public class SwerveDrive { private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); // ----- Simulation - private final ADXRS450_GyroSim gyroSim; + // TODO(Jade) WPILib doesn't have onboard IMU sim yet + // private final ADXRS450_GyroSim gyroSim; private final SwerveDriveSim swerveDriveSim; private double totalCurrentDraw = 0; @@ -91,7 +91,7 @@ public SwerveDrive() { visionStdDevs); // ----- Simulation - gyroSim = new ADXRS450_GyroSim(gyro); + // gyroSim = new ADXRS450_GyroSim(gyro); swerveDriveSim = new SwerveDriveSim( kDriveFF, @@ -117,13 +117,13 @@ public void periodic() { * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to * specific swerve module states. * - * @param vxMeters X velocity (forwards/backwards) - * @param vyMeters Y velocity (strafe left/right) - * @param omegaRadians Angular velocity (rotation CCW+) + * @param vx X velocity (forwards/backwards) + * @param vy Y velocity (strafe left/right) + * @param omega Angular velocity (rotation CCW+) */ - public void drive(double vxMeters, double vyMeters, double omegaRadians) { + public void drive(double vx, double vy, double omega) { var targetChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -189,8 +189,8 @@ public void resetPose(Pose2d pose, boolean resetSimPose) { for (int i = 0; i < swerveMods.length; i++) { swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); } - gyroSim.setAngle(-pose.getRotation().getDegrees()); - gyroSim.setRate(0); + // gyroSim.setAngle(-pose.getRotation().getDegrees()); + // gyroSim.setRate(0); } poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); @@ -267,14 +267,14 @@ public void log() { SmartDashboard.putNumber(table + "Y", pose.getY()); SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); - SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( - table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omega)); for (SwerveModule module : swerveMods) { module.log(); @@ -314,8 +314,8 @@ public void simulationPeriodic() { drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } - gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); - gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + // gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + // gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); } /** diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index ccef2bba6c..07bff21152 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -341,7 +341,7 @@ public void reset( driveStates.set(i, moduleDriveStates.get(i).copy()); steerStates.set(i, moduleSteerStates.get(i).copy()); } - omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omega; } /** @@ -444,12 +444,12 @@ public double getOmegaRadsPerSec() { */ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { - double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); // calculate battery current - return (inputVolts / battVolts) * (effVolts / motor.rOhms); + return (inputVolts / battVolts) * (effVolts / motor.R); } /** diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 5d77dcce51..6242a4e7ca 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -88,12 +88,12 @@ public void periodic() { steerMotor.setVoltage(steerPid); // Use feedforward model to translate target drive velocities into voltages - double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double driveFF = kDriveFF.calculate(desiredState.speed); double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -157,10 +157,10 @@ public void log() { SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speedMetersPerSecond)); + Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } From 7cfe1acd30438f159bba101e5ef4d4931baf190f Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 24 Jun 2025 00:25:30 -0400 Subject: [PATCH 10/15] Upgrade Gradle, fix build, and format --- .../src/main/include/subsystems/SwerveDrive.h | 6 +++--- .../frc/robot/subsystems/drivetrain/SwerveDrive.java | 6 ++---- .../frc/robot/subsystems/drivetrain/SwerveModule.java | 9 +++------ .../frc/robot/subsystems/drivetrain/SwerveDrive.java | 6 ++---- .../frc/robot/subsystems/drivetrain/SwerveModule.java | 9 +++------ .../java/frc/robot/subsystems/GamepieceLauncher.java | 3 +-- .../frc/robot/subsystems/drivetrain/SwerveDrive.java | 6 ++---- .../frc/robot/subsystems/drivetrain/SwerveModule.java | 9 +++------ 8 files changed, 19 insertions(+), 35 deletions(-) diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h index f47638fbdc..81e3d47bd9 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,9 +24,9 @@ #pragma once +#include #include #include -#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -72,8 +72,8 @@ class SwerveDrive { frc::SwerveDrivePoseEstimator<4> poseEstimator; frc::ChassisSpeeds targetChassisSpeeds{}; - //TODO(Jade) onboard imu doesn't have sim yet - // frc::sim::ADXRS450_GyroSim gyroSim; + // TODO(Jade) onboard imu doesn't have sim yet + // frc::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 871414f871..8e3fc97379 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public void periodic() { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public void log() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7ca..c393321713 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public void periodic() { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public void log() { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 7feea5c446..547d237bae 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public void periodic() { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public void log() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7ca..c393321713 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public void periodic() { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public void log() { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java index cb1a6ecb01..93e0fd661c 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -50,8 +50,7 @@ public void setRunning(boolean shouldRun) { } public void periodic() { - double maxRPM = - Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); + double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); motor.set(curMotorCmd); diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 60e1258029..cdb04cff63 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -122,8 +122,7 @@ public void periodic() { * @param omega Angular velocity (rotation CCW+) */ public void drive(double vx, double vy, double omega) { - var targetChassisSpeeds = - new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); + var targetChassisSpeeds = new ChassisSpeeds(vx, vy, omega).toRobotRelative(getHeading()); setChassisSpeeds(targetChassisSpeeds, true, false); } @@ -269,8 +268,7 @@ public void log() { ChassisSpeeds chassisSpeeds = getChassisSpeeds(); SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx); SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy); - SmartDashboard.putNumber( - table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); + SmartDashboard.putNumber(table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omega)); SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vx); SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vy); SmartDashboard.putNumber( diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 6242a4e7ca..c393321713 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -92,8 +92,7 @@ public void periodic() { double drivePid = 0; if (!openLoop) { // Perform PID feedback control to compensate for disturbances - drivePid = - drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); + drivePid = drivePidController.calculate(driveEncoder.getRate(), desiredState.speed); } driveMotor.setVoltage(driveFF + drivePid); @@ -156,11 +155,9 @@ public void log() { table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); SmartDashboard.putNumber( table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber(table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); SmartDashboard.putNumber( - table + "Drive Velocity Feet", Units.metersToFeet(state.speed)); - SmartDashboard.putNumber( - table + "Drive Velocity Target Feet", - Units.metersToFeet(desiredState.speed)); + table + "Drive Velocity Target Feet", Units.metersToFeet(desiredState.speed)); SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); } From 9f0246ea907367289a473c53a9a99b8a18c5f2c2 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Sat, 12 Jul 2025 00:03:01 -0400 Subject: [PATCH 11/15] Disable example builds --- .github/workflows/build.yml | 140 ++++++++++++++++++------------------ 1 file changed, 70 insertions(+), 70 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1597b74cfe..352860ae01 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -21,76 +21,76 @@ jobs: - uses: actions/checkout@v6 - uses: gradle/actions/wrapper-validation@v5 - build-examples: - - strategy: - fail-fast: false - matrix: - include: - - os: windows-2022 - artifact-name: Win64 - - os: macos-14 - artifact-name: macOS - - os: ubuntu-24.04 - artifact-name: Linux - - name: "Photonlib - Build Examples - ${{ matrix.os }}" - runs-on: ${{ matrix.os }} - needs: [build-photonlib-host, build-photonlib-docker] - - steps: - - name: Checkout code - uses: actions/checkout@v6 - with: - fetch-depth: 0 - - name: Fetch tags - run: git fetch --tags --force - - name: Install Java 17 - uses: actions/setup-java@v5 - with: - java-version: 17 - distribution: temurin - - name: Install SystemCore Toolchain - run: ./gradlew installSystemCoreToolchain - - name: Delete duplicate toolchains - run: | - find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} + - du -h . | sort -h - if: matrix.os == 'ubuntu-24.04' - # Download prebuilt photonlib artifacts - - uses: actions/download-artifact@v7 - with: - name: maven-${{ matrix.artifact-name }} - - uses: actions/download-artifact@v7 - with: - name: maven-Athena - - name: Move to maven local - run: | - mkdir -p ~/.m2/repository/ - mv maven/org ~/.m2/repository/ - - name: Copy vendordeps - shell: bash - run: | - for vendordep_folder in photonlib-*-examples/*/; do - # Remove trailing slash for cross-platform compatibility - vendordep_folder="${vendordep_folder%/}" - - # Filter for projects only - if [ -e "$vendordep_folder/build.gradle" ]; then - mkdir -p "$vendordep_folder/vendordeps/" - cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/" - fi - done - - name: Build Java examples - working-directory: photonlib-java-examples - run: | - ./gradlew build - ./gradlew clean - - name: Build C++ examples - working-directory: photonlib-cpp-examples - run: | - ./gradlew build - ./gradlew clean + # build-examples: + + # strategy: + # fail-fast: false + # matrix: + # include: + # - os: windows-2022 + # artifact-name: Win64 + # - os: macos-14 + # artifact-name: macOS + # - os: ubuntu-24.04 + # artifact-name: Linux + + # name: "Photonlib - Build Examples - ${{ matrix.os }}" + # runs-on: ${{ matrix.os }} + # needs: [build-photonlib-host, build-photonlib-docker] + + # steps: + # - name: Checkout code + # uses: actions/checkout@v6 + # with: + # fetch-depth: 0 + # - name: Fetch tags + # run: git fetch --tags --force + # - name: Install Java 17 + # uses: actions/setup-java@v5 + # with: + # java-version: 17 + # distribution: temurin + # - name: Install SystemCore Toolchain + # run: ./gradlew installSystemCoreToolchain + # - name: Delete duplicate toolchains + # run: | + # find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} + + # du -h . | sort -h + # if: matrix.os == 'ubuntu-24.04' + # # Download prebuilt photonlib artifacts + # - uses: actions/download-artifact@v7 + # with: + # name: maven-${{ matrix.artifact-name }} + # - uses: actions/download-artifact@v7 + # with: + # name: maven-Athena + # - name: Move to maven local + # run: | + # mkdir -p ~/.m2/repository/ + # mv maven/org ~/.m2/repository/ + # - name: Copy vendordeps + # shell: bash + # run: | + # for vendordep_folder in photonlib-*-examples/*/; do + # # Remove trailing slash for cross-platform compatibility + # vendordep_folder="${vendordep_folder%/}" + + # # Filter for projects only + # if [ -e "$vendordep_folder/build.gradle" ]; then + # mkdir -p "$vendordep_folder/vendordeps/" + # cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/" + # fi + # done + # - name: Build Java examples + # working-directory: photonlib-java-examples + # run: | + # ./gradlew build + # ./gradlew clean + # - name: Build C++ examples + # working-directory: photonlib-cpp-examples + # run: | + # ./gradlew build + # ./gradlew clean playwright-tests: name: "Playwright E2E tests" From 1965f81c3a4e56ce651c56ee73c027e2f9235cde Mon Sep 17 00:00:00 2001 From: Jordan McMichael Date: Sat, 12 Jul 2025 00:35:09 -0400 Subject: [PATCH 12/15] [2027] Add systemcore as a photonlib build target (#1995) ## Description Added systemcore to a couple of build files in order for `./gradlew publishToMavenLocal` to generate systemcore-compatible dependencies. Needed to support deploying photonlib to systemcore. ## Meta Merge checklist: - [x] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes - [x] The description documents the _what_ and _why_ - [ ] If this PR changes behavior or adds a feature, user documentation is updated - [ ] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly - [ ] If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1 - [ ] If this PR touches pipeline settings or anything related to data exchange, the frontend typing is updated - [ ] If this PR addresses a bug, a regression test for it is added --------- Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com> --- .github/workflows/build.yml | 3 +++ photon-lib/src/generate/photonlib.json.in | 6 +++--- photon-targeting/build.gradle | 2 +- shared/config.gradle | 1 + 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 352860ae01..499edef63f 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -265,6 +265,9 @@ jobs: fail-fast: false matrix: include: + - container: wpilib/systemcore-cross-ubuntu:2025-24.04 + artifact-name: SystemCore + build-options: "-Ponlylinuxsystemcore" - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 artifact-name: Raspbian build-options: "-Ponlylinuxarm32" diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in index 5df9ae5d2c..a1a62268ea 100644 --- a/photon-lib/src/generate/photonlib.json.in +++ b/photon-lib/src/generate/photonlib.json.in @@ -18,7 +18,7 @@ "isJar": false, "validPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] @@ -35,7 +35,7 @@ "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] @@ -50,7 +50,7 @@ "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 7bf667bcc9..ed132739a3 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -66,7 +66,7 @@ model { enableCheckTask project.hasProperty('doJniCheck') javaCompileTasks << compileJava - jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio) + jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) diff --git a/shared/config.gradle b/shared/config.gradle index fbb48de0af..ebc32468be 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -2,6 +2,7 @@ nativeUtils.addWpiNativeUtils() nativeUtils.withCrossLinuxArm32() nativeUtils.withCrossLinuxArm64() +nativeUtils.withCrossSystemCore() // Configure WPI dependencies. nativeUtils.wpi.configureDependencies { From 1ccf0f469e18e8a7c6b8f5464ca5d47012589430 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Sat, 19 Jul 2025 15:36:10 -0400 Subject: [PATCH 13/15] Add DataLog to the list of libraries loaded --- .../src/main/java/org/photonvision/jni/LibraryLoader.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java b/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java index 97f686c8b5..50f855464a 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java @@ -20,6 +20,7 @@ import edu.wpi.first.apriltag.jni.AprilTagJNI; import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.cscore.OpenCvLoader; +import edu.wpi.first.datalog.DataLogJNI; import edu.wpi.first.hal.JNIWrapper; import edu.wpi.first.math.jni.WPIMathJNI; import edu.wpi.first.net.WPINetJNI; @@ -37,6 +38,7 @@ public static boolean loadWpiLibraries() { NetworkTablesJNI.Helper.setExtractOnStaticLoad(false); WPIUtilJNI.Helper.setExtractOnStaticLoad(false); + DataLogJNI.Helper.setExtractOnStaticLoad(false); CameraServerJNI.Helper.setExtractOnStaticLoad(false); OpenCvLoader.Helper.setExtractOnStaticLoad(false); JNIWrapper.Helper.setExtractOnStaticLoad(false); @@ -54,6 +56,7 @@ public static boolean loadWpiLibraries() { "wpinetjni", "wpiHaljni", "cscorejni", + "datalogjni", "apriltagjni"); CombinedRuntimeLoader.loadLibraries(LibraryLoader.class, Core.NATIVE_LIBRARY_NAME); From 3a62eea545b8c8581d14d4294ea7c3c8a0ef530f Mon Sep 17 00:00:00 2001 From: Jordan McMichael Date: Tue, 22 Jul 2025 22:46:46 -0400 Subject: [PATCH 14/15] Upgrade to 2027 alpha 2 (#2010) --- build.gradle | 4 ++-- photonlib-cpp-examples/aimandrange/build.gradle | 2 +- photonlib-cpp-examples/aimattarget/build.gradle | 2 +- photonlib-cpp-examples/poseest/build.gradle | 2 +- photonlib-java-examples/aimandrange/build.gradle | 2 +- photonlib-java-examples/aimattarget/build.gradle | 2 +- photonlib-java-examples/poseest/build.gradle | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 232f450829..4971696120 100644 --- a/build.gradle +++ b/build.gradle @@ -4,7 +4,7 @@ plugins { id "cpp" id "com.diffplug.spotless" version "8.1.0" id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2025.0" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" id 'org.photonvision.tools.WpilibTools' version '2.4.1-photon' id 'com.google.protobuf' version '0.9.3' apply false id 'edu.wpi.first.GradleJni' version '1.1.0' @@ -33,7 +33,7 @@ ext.allOutputsFolder = file("$project.buildDir/outputs") apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2027.0.0-alpha-1" + wpilibVersion = "2027.0.0-alpha-2" wpimathVersion = wpilibVersion openCVYear = "2025" openCVversion = "4.10.0-3" diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index 4c6930e3b7..b6222c17d7 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } repositories { diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 4c6930e3b7..b6222c17d7 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } repositories { diff --git a/photonlib-cpp-examples/poseest/build.gradle b/photonlib-cpp-examples/poseest/build.gradle index 4c6930e3b7..b6222c17d7 100644 --- a/photonlib-cpp-examples/poseest/build.gradle +++ b/photonlib-cpp-examples/poseest/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } repositories { diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 131232caad..b6983ba2d8 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } java { diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index 4622163ee8..5780d66e7c 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } java { diff --git a/photonlib-java-examples/poseest/build.gradle b/photonlib-java-examples/poseest/build.gradle index 4622163ee8..5780d66e7c 100644 --- a/photonlib-java-examples/poseest/build.gradle +++ b/photonlib-java-examples/poseest/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-1" + id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" } java { From f640d8db0b9cd8fb20ccc5209f4a644021e567cf Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Mon, 29 Dec 2025 16:16:56 -0500 Subject: [PATCH 15/15] Update to match new WPILib organization --- build.gradle | 10 +- .../docs/apriltag-pipelines/multitag.md | 2 +- .../photonlib/driver-mode-pipeline-index.md | 2 +- .../photonlib/getting-target-data.md | 10 +- .../photonlib/using-target-data.md | 8 +- docs/source/docs/quick-start/networking.md | 2 +- .../configuration/CameraConfiguration.java | 2 +- .../configuration/LegacyConfigProvider.java | 4 +- .../configuration/PhotonConfiguration.java | 2 +- .../configuration/SqlConfigProvider.java | 6 +- .../networktables/NTDataChangeListener.java | 6 +- .../networktables/NTDataPublisher.java | 11 +- .../networktables/NTDriverStation.java | 14 +- .../networktables/NetworkTablesManager.java | 24 +- .../networktables/TimeSyncManager.java | 10 +- .../dataflow/websocket/UIProgramSettings.java | 2 +- .../common/hardware/HardwareManager.java | 5 +- .../common/hardware/VisionLED.java | 2 +- .../hardware/metrics/SystemMonitor.java | 6 +- .../metrics/proto/DeviceMetricsProto.java | 2 +- .../photonvision/common/logging/Logger.java | 2 +- .../common/logging/PvCSCoreLogger.java | 2 +- .../common/networking/NetworkUtils.java | 2 +- .../common/util/SerializationUtils.java | 2 +- .../photonvision/common/util/TestUtils.java | 5 +- .../common/util/math/MathUtils.java | 18 +- .../vision/calibration/BoardObservation.java | 7 +- .../vision/calibration/JsonMatOfDouble.java | 4 +- .../vision/camera/FileVisionSource.java | 6 +- .../vision/camera/PVCameraInfo.java | 2 +- .../USBCameras/ArduOV2311CameraSettables.java | 2 +- .../ArduOV2311WindowsCameraSettables.java | 7 +- .../USBCameras/ArduOV9281CameraSettables.java | 2 +- .../USBCameras/ArduOV9782CameraSettables.java | 2 +- .../USBCameras/GenericUSBCameraSettables.java | 15 +- .../USBCameras/InnoOV9281CameraSettables.java | 2 +- .../USBCameras/LifeCam3kCameraSettables.java | 7 +- .../LifeCam3kWindowsCameraSettables.java | 7 +- .../USBCameras/PsEyeCameraSettables.java | 2 +- .../USBCameras/See3Cam24CUGSettables.java | 2 +- .../camera/USBCameras/USBCameraSource.java | 8 +- .../camera/csi/LibcameraGpuSettables.java | 11 +- .../vision/camera/csi/LibcameraGpuSource.java | 4 +- .../vision/frame/FrameStaticProperties.java | 2 +- .../frame/consumer/FileSaveFrameConsumer.java | 10 +- .../frame/consumer/MJPGFrameConsumer.java | 6 +- .../frame/provider/USBFrameProvider.java | 15 +- .../org/photonvision/vision/opencv/CVMat.java | 2 +- .../vision/opencv/ImageRotationMode.java | 6 +- .../pipe/impl/AprilTagDetectionPipe.java | 4 +- .../pipe/impl/AprilTagPoseEstimatorPipe.java | 8 +- .../pipe/impl/ArucoPoseEstimatorPipe.java | 10 +- .../vision/pipe/impl/CalculateFPSPipe.java | 4 +- .../vision/pipe/impl/Draw2dCrosshairPipe.java | 2 +- .../vision/pipe/impl/Draw2dTargetsPipe.java | 2 +- .../vision/pipe/impl/Draw3dTargetsPipe.java | 2 +- .../vision/pipe/impl/DrawCalibrationPipe.java | 2 +- .../pipe/impl/FindBoardCornersPipe.java | 2 +- .../vision/pipe/impl/FindCirclesPipe.java | 2 +- .../vision/pipe/impl/MultiTargetPNPPipe.java | 2 +- .../vision/pipe/impl/SolvePNPPipe.java | 8 +- .../vision/pipeline/AprilTagPipeline.java | 18 +- .../vision/pipeline/ArucoPipeline.java | 10 +- .../vision/pipeline/Calibrate3dPipeline.java | 4 +- .../Calibration3dPipelineSettings.java | 2 +- .../vision/pipeline/ColoredShapePipeline.java | 2 +- .../vision/pipeline/DriverModePipeline.java | 2 +- .../vision/pipeline/OutputStreamPipeline.java | 2 +- .../pipeline/result/CVPipelineResult.java | 6 +- .../vision/processes/VisionModule.java | 6 +- .../VisionModuleChangeSubscriber.java | 2 +- .../vision/processes/VisionSourceManager.java | 2 +- .../processes/VisionSourceSettables.java | 2 +- .../vision/target/TargetModel.java | 2 +- .../vision/target/TrackedTarget.java | 6 +- .../common/configuration/SQLConfigTest.java | 2 +- .../common/util/CoordinateConversionTest.java | 8 +- .../consumer/FileSaveFrameConsumerTest.java | 14 +- .../vision/pipeline/AprilTagTest.java | 2 +- .../vision/pipeline/ArucoPipelineTest.java | 2 +- .../vision/pipeline/Calibrate3dPipeTest.java | 2 +- .../vision/pipeline/SolvePNPTest.java | 6 +- .../vision/processes/MockUsbCameraSource.java | 4 +- .../processes/VisionModuleManagerTest.java | 2 +- .../processes/VisionSourceManagerTest.java | 2 +- .../vision/target/TargetCalculationsTest.java | 4 +- photon-lib/build.gradle | 4 +- .../org/photonvision/EstimatedRobotPose.java | 2 +- .../java/org/photonvision/PhotonCamera.java | 40 +- .../org/photonvision/PhotonPoseEstimator.java | 34 +- .../java/org/photonvision/PhotonUtils.java | 4 +- .../simulation/PhotonCameraSim.java | 29 +- .../simulation/SimCameraProperties.java | 27 +- .../photonvision/simulation/VideoSimUtil.java | 14 +- .../simulation/VisionSystemSim.java | 18 +- .../simulation/VisionTargetSim.java | 4 +- .../timesync/TimeSyncSingleton.java | 2 +- .../main/native/cpp/photon/PhotonCamera.cpp | 79 ++-- .../native/cpp/photon/PhotonPoseEstimator.cpp | 236 +++++------ .../cpp/photon/simulation/PhotonCameraSim.cpp | 81 ++-- .../photon/simulation/SimCameraProperties.cpp | 86 ++-- .../main/native/include/photon/PhotonCamera.h | 77 ++-- .../include/photon/PhotonPoseEstimator.h | 77 ++-- .../main/native/include/photon/PhotonUtils.h | 94 +++-- .../photon/simulation/PhotonCameraSim.h | 41 +- .../photon/simulation/SimCameraProperties.h | 99 ++--- .../include/photon/simulation/VideoSimUtil.h | 127 +++--- .../photon/simulation/VisionSystemSim.h | 120 +++--- .../photon/simulation/VisionTargetSim.h | 41 +- .../LegacyPhotonPoseEstimatorTest.java | 34 +- .../java/org/photonvision/OpenCVTest.java | 14 +- .../org/photonvision/PhotonCameraTest.java | 20 +- .../photonvision/PhotonPoseEstimatorTest.java | 32 +- .../java/org/photonvision/PhotonUtilTest.java | 4 +- .../org/photonvision/VisionSystemSimTest.java | 30 +- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 272 ++++++------- .../src/test/native/cpp/PhotonCameraTest.cpp | 10 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 381 +++++++++--------- .../test/native/cpp/VisionSystemSimTest.cpp | 380 +++++++++-------- photon-lib/src/test/native/cpp/main.cpp | 2 +- photon-serde/generate_messages.py | 2 +- photon-serde/messages.yaml | 6 +- photon-serde/templates/Message.java.jinja | 2 +- photon-serde/templates/ThingSerde.h.jinja | 2 +- photon-server/build.gradle | 6 +- .../src/main/java/org/photonvision/Main.java | 3 +- .../server/DataSocketHandler.java | 2 +- photon-targeting/build.gradle | 7 +- .../struct/PhotonPipelineResultSerde.java | 2 +- .../struct/PhotonTrackedTargetSerde.java | 2 +- .../photonvision/struct/PnpResultSerde.java | 2 +- .../photon/serde/PhotonTrackedTargetSerde.cpp | 8 +- .../cpp/photon/serde/PnpResultSerde.cpp | 8 +- .../photon/serde/MultiTargetPNPResultSerde.h | 2 +- .../serde/PhotonPipelineMetadataSerde.h | 2 +- .../photon/serde/PhotonPipelineResultSerde.h | 2 +- .../photon/serde/PhotonTrackedTargetSerde.h | 4 +- .../include/photon/serde/PnpResultSerde.h | 4 +- .../include/photon/serde/TargetCornerSerde.h | 2 +- .../photon/struct/PhotonTrackedTargetStruct.h | 6 +- .../include/photon/struct/PnpResultStruct.h | 6 +- .../dataflow/structures/PacketSerde.java | 2 +- .../common/networktables/NTTopicSet.java | 26 +- .../common/networktables/PacketPublisher.java | 2 +- .../networktables/PacketSubscriber.java | 2 +- .../estimation/CameraTargetRelation.java | 6 +- .../photonvision/estimation/OpenCVHelp.java | 21 +- .../estimation/RotTrlTransform3d.java | 8 +- .../photonvision/estimation/TargetModel.java | 10 +- .../estimation/VisionEstimation.java | 24 +- .../org/photonvision/jni/CscoreExtras.java | 6 +- .../org/photonvision/jni/LibraryLoader.java | 18 +- .../org/photonvision/jni/TimeSyncClient.java | 11 +- .../targeting/MultiTargetPNPResult.java | 2 +- .../targeting/PhotonPipelineMetadata.java | 2 +- .../targeting/PhotonPipelineResult.java | 8 +- .../targeting/PhotonTrackedTarget.java | 4 +- .../org/photonvision/targeting/PnpResult.java | 4 +- .../photonvision/targeting/TargetCorner.java | 2 +- .../proto/MultiTargetPNPResultProto.java | 2 +- .../targeting/proto/PNPResultProto.java | 4 +- .../proto/PhotonPipelineResultProto.java | 2 +- .../proto/PhotonTrackedTargetProto.java | 4 +- .../targeting/proto/TargetCornerProto.java | 2 +- .../org/photonvision/utils/PacketUtils.java | 2 +- .../main/native/cpp/net/TimeSyncClient.cpp | 63 +-- .../main/native/cpp/net/TimeSyncServer.cpp | 41 +- .../wrap/casadi_wrapper.cpp | 26 +- .../photon/estimation/VisionEstimation.cpp | 51 +-- .../main/native/include/net/TimeSyncClient.h | 26 +- .../main/native/include/net/TimeSyncServer.h | 16 +- .../main/native/include/net/TimeSyncStructs.h | 38 +- .../wrap/casadi_wrapper.h | 4 +- .../photon/dataflow/structures/Packet.h | 14 +- .../photon/estimation/CameraTargetRelation.h | 54 +-- .../include/photon/estimation/OpenCVHelp.h | 60 +-- .../photon/estimation/RotTrlTransform3d.h | 65 +-- .../include/photon/estimation/TargetModel.h | 77 ++-- .../photon/estimation/VisionEstimation.h | 19 +- .../include/photon/networktables/NTTopicSet.h | 61 +-- .../photon/targeting/PhotonPipelineResult.h | 12 +- .../photon/targeting/PhotonTrackedTarget.h | 8 +- .../native/jni/ConstrainedSolvepnpJNI.cpp | 15 +- .../src/main/native/jni/CscoreExtras.cpp | 14 +- .../main/native/jni/FileLoggerExtrasJNI.cpp | 6 +- .../src/main/native/jni/jni_utils.h | 18 +- .../test/java/ConstrainedSolvepnpTest.java | 8 +- .../src/test/java/jni/CscoreExtrasTest.java | 18 +- .../src/test/java/jni/FileLoggerTest.java | 4 +- .../src/test/java/net/TimeSyncTest.java | 4 +- .../java/org/photonvision/PacketTest.java | 2 +- .../targeting/MultiTargetPNPResultTest.java | 6 +- .../photonvision/targeting/PNPResultTest.java | 4 +- .../targeting/PhotonPipelineResultTest.java | 6 +- .../targeting/PhotonTrackedTargetTest.java | 6 +- .../proto/MultiTargetPNPResultProtoTest.java | 6 +- .../targeting/proto/PNPResultProtoTest.java | 4 +- .../proto/PhotonPipelineResultProtoTest.java | 6 +- .../proto/PhotonTrackedTargetProtoTest.java | 6 +- .../src/test/native/cpp/CasadiWrapperTest.cpp | 8 +- .../src/test/native/cpp/PacketTest.cpp | 42 +- photon-targeting/src/test/native/cpp/main.cpp | 2 +- .../src/test/native/cpp/net/TimeSyncTest.cpp | 6 +- .../aimandrange/src/main/cpp/Robot.cpp | 26 +- .../src/main/cpp/subsystems/SwerveDrive.cpp | 123 +++--- .../main/cpp/subsystems/SwerveDriveSim.cpp | 144 +++---- .../src/main/cpp/subsystems/SwerveModule.cpp | 90 ++--- .../aimandrange/src/main/include/Constants.h | 65 +-- .../aimandrange/src/main/include/Robot.h | 8 +- .../aimandrange/src/main/include/VisionSim.h | 16 +- .../src/main/include/subsystems/SwerveDrive.h | 48 +-- .../main/include/subsystems/SwerveDriveSim.h | 103 ++--- .../main/include/subsystems/SwerveModule.h | 74 ++-- .../aimandrange/src/test/cpp/main.cpp | 2 +- .../aimattarget/src/main/cpp/Robot.cpp | 18 +- .../src/main/cpp/subsystems/SwerveDrive.cpp | 123 +++--- .../main/cpp/subsystems/SwerveDriveSim.cpp | 144 +++---- .../src/main/cpp/subsystems/SwerveModule.cpp | 90 ++--- .../aimattarget/src/main/include/Constants.h | 65 +-- .../aimattarget/src/main/include/Robot.h | 8 +- .../aimattarget/src/main/include/VisionSim.h | 16 +- .../src/main/include/subsystems/SwerveDrive.h | 46 +-- .../main/include/subsystems/SwerveDriveSim.h | 103 ++--- .../main/include/subsystems/SwerveModule.h | 74 ++-- .../aimattarget/src/test/cpp/main.cpp | 2 +- .../poseest/src/main/cpp/Robot.cpp | 20 +- .../main/cpp/subsystems/GamepieceLauncher.cpp | 10 +- .../src/main/cpp/subsystems/SwerveDrive.cpp | 135 ++++--- .../main/cpp/subsystems/SwerveDriveSim.cpp | 144 +++---- .../src/main/cpp/subsystems/SwerveModule.cpp | 90 ++--- .../poseest/src/main/include/Constants.h | 65 +-- .../poseest/src/main/include/Robot.h | 10 +- .../poseest/src/main/include/Vision.h | 28 +- .../include/subsystems/GamepieceLauncher.h | 29 +- .../src/main/include/subsystems/SwerveDrive.h | 54 +-- .../main/include/subsystems/SwerveDriveSim.h | 103 ++--- .../main/include/subsystems/SwerveModule.h | 74 ++-- .../poseest/src/test/cpp/main.cpp | 2 +- .../src/main/java/frc/robot/Constants.java | 24 +- .../src/main/java/frc/robot/Main.java | 2 +- .../src/main/java/frc/robot/Robot.java | 16 +- .../src/main/java/frc/robot/VisionSim.java | 6 +- .../subsystems/drivetrain/SwerveDrive.java | 30 +- .../subsystems/drivetrain/SwerveDriveSim.java | 53 ++- .../subsystems/drivetrain/SwerveModule.java | 22 +- .../src/main/java/frc/robot/Constants.java | 24 +- .../src/main/java/frc/robot/Main.java | 2 +- .../src/main/java/frc/robot/Robot.java | 14 +- .../src/main/java/frc/robot/VisionSim.java | 6 +- .../subsystems/drivetrain/SwerveDrive.java | 30 +- .../subsystems/drivetrain/SwerveDriveSim.java | 53 ++- .../subsystems/drivetrain/SwerveModule.java | 22 +- .../src/test/java/frc/robot/JniLoadTest.java | 4 +- .../src/main/java/frc/robot/Constants.java | 24 +- .../poseest/src/main/java/frc/robot/Main.java | 2 +- .../src/main/java/frc/robot/Robot.java | 16 +- .../src/main/java/frc/robot/Vision.java | 20 +- .../robot/subsystems/GamepieceLauncher.java | 17 +- .../subsystems/drivetrain/SwerveDrive.java | 30 +- .../subsystems/drivetrain/SwerveDriveSim.java | 53 ++- .../subsystems/drivetrain/SwerveModule.java | 22 +- shared/common.gradle | 23 +- shared/config.gradle | 1 - shared/javacommon.gradle | 22 +- 264 files changed, 3440 insertions(+), 3299 deletions(-) diff --git a/build.gradle b/build.gradle index 4971696120..b5f8a51e1d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,13 +1,13 @@ -import edu.wpi.first.toolchain.* +import org.wpilib.toolchain.* plugins { id "cpp" id "com.diffplug.spotless" version "8.1.0" - id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2025.0" - id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2" + id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-2" id 'org.photonvision.tools.WpilibTools' version '2.4.1-photon' id 'com.google.protobuf' version '0.9.3' apply false - id 'edu.wpi.first.GradleJni' version '1.1.0' + id 'org.wpilib.GradleJni' version '2027.0.0' id "org.ysb33r.doxygen" version "2.0.0" apply false id 'com.gradleup.shadow' version '8.3.4' apply false id "com.github.node-gradle.node" version "7.0.1" apply false @@ -33,7 +33,7 @@ ext.allOutputsFolder = file("$project.buildDir/outputs") apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2027.0.0-alpha-2" + wpilibVersion = "2027.0.0-alpha-3-203-g0001ddc7e" wpimathVersion = wpilibVersion openCVYear = "2025" openCVversion = "4.10.0-3" diff --git a/docs/source/docs/apriltag-pipelines/multitag.md b/docs/source/docs/apriltag-pipelines/multitag.md index 1a01ff03f1..f33e3dfb73 100644 --- a/docs/source/docs/apriltag-pipelines/multitag.md +++ b/docs/source/docs/apriltag-pipelines/multitag.md @@ -46,7 +46,7 @@ This multi-target pose estimate can be accessed using PhotonLib. We suggest usin { auto multiTagResult = result.MultiTagResult(); if (multiTagResult.has_value()) { - frc::Transform3d fieldToCamera = multiTagResult->estimatedPose.best; + wpi::math::Transform3d fieldToCamera = multiTagResult->estimatedPose.best; } } diff --git a/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md b/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md index fbb22095a7..65a1b5f8f3 100644 --- a/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md +++ b/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md @@ -60,7 +60,7 @@ You can also get the pipeline latency from a pipeline result using the `getLaten .. code-block:: c++ // Get the pipeline latency. - units::second_t latency = result.GetLatency(); + wpi::units::second_t latency = result.GetLatency(); .. code-block:: python diff --git a/docs/source/docs/programming/photonlib/getting-target-data.md b/docs/source/docs/programming/photonlib/getting-target-data.md index 16a7892854..c9dd9f02ae 100644 --- a/docs/source/docs/programming/photonlib/getting-target-data.md +++ b/docs/source/docs/programming/photonlib/getting-target-data.md @@ -107,7 +107,7 @@ You can get a list of tracked targets using the `getTargets()`/`GetTargets()` (J .. code-block:: c++ // Get a list of currently tracked targets. - wpi::ArrayRef targets = result.GetTargets(); + std::span targets = result.GetTargets(); .. code-block:: python @@ -166,8 +166,8 @@ You can get the {ref}`best target , 4> corners = target.GetCorners(); + wpi::math::Transform2d pose = target.GetCameraToTarget(); + wpi::util::SmallVector, 4> corners = target.GetCorners(); .. code-block:: python @@ -206,8 +206,8 @@ All of the data above (**except skew**) is available when using AprilTags. // Get information from target. int targetID = target.GetFiducialId(); double poseAmbiguity = target.GetPoseAmbiguity(); - frc::Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - frc::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); + wpi::math::Transform3d bestCameraToTarget = target.getBestCameraToTarget(); + wpi::math::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); .. code-block:: python diff --git a/docs/source/docs/programming/photonlib/using-target-data.md b/docs/source/docs/programming/photonlib/using-target-data.md index 657b6ba3e1..b8591a9736 100644 --- a/docs/source/docs/programming/photonlib/using-target-data.md +++ b/docs/source/docs/programming/photonlib/using-target-data.md @@ -38,8 +38,8 @@ You can get your robot's `Pose2D` on the field using various camera data, target .. code-block:: c++ // Calculate robot's field relative pose - frc::Pose2D robotPose = photonlib::EstimateFieldToRobot( - kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot); + wpi::math::Pose2d robotPose = photonlib::EstimateFieldToRobot( + kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, wpi::math::Rotation2d(wpi::units::degree_t(-target.GetYaw())), wpi::math::Rotation2d(wpi::units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot); .. code-block:: python @@ -106,8 +106,8 @@ You can get a [translation](https://docs.wpilib.org/en/latest/docs/software/adva .. code-block:: c++ // Calculate a translation from the camera to the target. - frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation( - distance, frc::Rotation2d(units::degree_t(-target.GetYaw()))); + wpi::math::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation( + distance, wpi::math::Rotation2d(wpi::units::degree_t(-target.GetYaw()))); .. code-block:: python diff --git a/docs/source/docs/quick-start/networking.md b/docs/source/docs/quick-start/networking.md index e373221dc7..6a0433aa77 100644 --- a/docs/source/docs/quick-start/networking.md +++ b/docs/source/docs/quick-start/networking.md @@ -81,7 +81,7 @@ If you would like to access your Ethernet-connected vision device from a compute .. code-block:: c++ - wpi::PortForwarder::GetInstance().Add(5800, "photonvision.local", 5800); + wpi::net::PortForwarder::GetInstance().Add(5800, "photonvision.local", 5800); .. code-block:: python diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index e51175f36c..9c479bd7d0 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -20,7 +20,6 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.cscore.UsbCameraInfo; import java.util.ArrayList; import java.util.List; import java.util.UUID; @@ -35,6 +34,7 @@ import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.DriverModePipelineSettings; import org.photonvision.vision.processes.PipelineManager; +import org.wpilib.vision.camera.UsbCameraInfo; public class CameraConfiguration { private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera); diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java index 28de811a91..5b51972564 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java @@ -18,8 +18,6 @@ package org.photonvision.common.configuration; import com.fasterxml.jackson.core.JsonProcessingException; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import java.io.File; import java.io.IOException; import java.io.UncheckedIOException; @@ -40,6 +38,8 @@ import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.DriverModePipelineSettings; import org.photonvision.vision.processes.VisionSource; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; import org.zeroturnaround.zip.ZipUtil; class LegacyConfigProvider extends ConfigProvider { diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index 64bd754449..83ece829c7 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -17,11 +17,11 @@ package org.photonvision.common.configuration; -import edu.wpi.first.apriltag.AprilTagFieldLayout; import java.util.Collection; import java.util.HashMap; import java.util.List; import org.photonvision.vision.processes.VisionSource; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; public class PhotonConfiguration { private final HardwareConfig hardwareConfig; diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 25fff42875..c00e98e33d 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -17,9 +17,6 @@ package org.photonvision.common.configuration; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.cscore.UsbCameraInfo; import java.io.File; import java.io.IOException; import java.io.UncheckedIOException; @@ -40,6 +37,9 @@ import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.DriverModePipelineSettings; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; +import org.wpilib.vision.camera.UsbCameraInfo; /** * Saves settings in a SQLite database file (called photon.sqlite). diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java index 2bd172cadb..2d3ae4df78 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java @@ -17,11 +17,11 @@ package org.photonvision.common.dataflow.networktables; -import edu.wpi.first.networktables.NetworkTableEvent; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.Subscriber; import java.util.EnumSet; import java.util.function.Consumer; +import org.wpilib.networktables.NetworkTableEvent; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.Subscriber; public class NTDataChangeListener { private final NetworkTableInstance instance; diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 7eff9733a0..53e7cba3a4 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -17,10 +17,6 @@ package org.photonvision.common.dataflow.networktables; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEvent; -import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; @@ -35,6 +31,10 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.pipeline.result.CalibrationPipelineResult; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.NetworkTableEvent; +import org.wpilib.networktables.NetworkTablesJNI; public class NTDataPublisher implements CVPipelineResultConsumer { private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General); @@ -177,7 +177,8 @@ public void accept(CVPipelineResult result) { var offset = NetworkTablesManager.getInstance().getOffset(); - // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's + // Transform the metadata timestamps from the local wpi::nt::Now timebase to the Time Sync + // Server's // timebase var simplified = new PhotonPipelineResult( diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java index 0e48008e88..5001972395 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java @@ -17,15 +17,15 @@ package org.photonvision.common.dataflow.networktables; -import edu.wpi.first.networktables.BooleanSubscriber; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEvent.Kind; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringSubscriber; import java.util.EnumSet; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.wpilib.networktables.BooleanSubscriber; +import org.wpilib.networktables.IntegerSubscriber; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.NetworkTableEvent.Kind; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.StringSubscriber; // Helper to print when the robot transitions modes public class NTDriverStation { @@ -125,7 +125,7 @@ private void printMatchData() { } // Copied from - // https://github.com/wpilibsuite/allwpilib/blob/07192285f65321a2f7363227a2216f09b715252d/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java#L123C1-L140C4 + // https://github.com/wpilibsuite/allwpilib/blob/07192285f65321a2f7363227a2216f09b715252d/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java#L123C1-L140C4 // TODO: upstream! /** * Gets the current control word of the driver station. diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index bad248e26d..0307e2fb27 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -17,18 +17,6 @@ package org.photonvision.common.dataflow.networktables; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.networktables.LogMessage; -import edu.wpi.first.networktables.MultiSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEvent; -import edu.wpi.first.networktables.NetworkTableEvent.Kind; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringSubscriber; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.Arrays; import java.util.EnumSet; @@ -47,6 +35,18 @@ import org.photonvision.common.networking.NetworkUtils; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.common.util.file.JacksonUtils; +import org.wpilib.networktables.LogMessage; +import org.wpilib.networktables.MultiSubscriber; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.NetworkTableEvent; +import org.wpilib.networktables.NetworkTableEvent.Kind; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.StringSubscriber; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.AlertType; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.camera.CameraServerJNI; public class NetworkTablesManager { private static final Logger logger = diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java index a850109365..478a794de0 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java @@ -17,16 +17,16 @@ package org.photonvision.common.dataflow.networktables; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import org.photonvision.common.configuration.NetworkConfig; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.jni.TimeSyncClient; import org.photonvision.jni.TimeSyncServer; +import org.wpilib.networktables.IntegerPublisher; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.vision.camera.CameraServerJNI; public class TimeSyncManager { private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables); @@ -66,7 +66,7 @@ public void start() { public synchronized long getOffset() { // if we're a client, return the offset to server time if (m_client != null) return m_client.getOffset(); - // if we're a server, our time (nt::Now) is the same as network time + // if we're a server, our time (wpi::nt::Now) is the same as network time if (m_server != null) return 0; // ????? should never hit diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIProgramSettings.java b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIProgramSettings.java index 16710a1b7b..7b55ab0a0f 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIProgramSettings.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIProgramSettings.java @@ -17,7 +17,7 @@ package org.photonvision.common.dataflow.websocket; -import edu.wpi.first.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; public class UIProgramSettings { public UIProgramSettings( diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index 2da2173be3..e097f2926d 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -21,8 +21,6 @@ import com.diozero.internal.spi.NativeDeviceFactoryInterface; import com.diozero.sbc.BoardPinInfo; import com.diozero.sbc.DeviceFactoryHelper; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.IntegerSubscriber; import java.io.IOException; import java.util.HashSet; import java.util.List; @@ -38,6 +36,9 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; +import org.photonvision.common.util.TimedTaskManager; +import org.wpilib.networktables.IntegerPublisher; +import org.wpilib.networktables.IntegerSubscriber; public class HardwareManager { private static HardwareManager instance; diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java index bc4b3b6c17..926091ca83 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java @@ -21,7 +21,6 @@ import com.diozero.devices.PwmLed; import com.diozero.internal.spi.NativeDeviceFactoryInterface; import com.diozero.sbc.BoardPinInfo; -import edu.wpi.first.networktables.NetworkTableEvent; import java.util.ArrayList; import java.util.List; import java.util.concurrent.atomic.AtomicInteger; @@ -31,6 +30,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.common.util.math.MathUtils; +import org.wpilib.networktables.NetworkTableEvent; public class VisionLED implements AutoCloseable { private static final Logger logger = new Logger(VisionLED.class, LogGroup.VisionModule); diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/SystemMonitor.java b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/SystemMonitor.java index a9401649be..79b3a702e5 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/SystemMonitor.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/SystemMonitor.java @@ -17,9 +17,6 @@ package org.photonvision.common.hardware.metrics; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.ProtobufPublisher; import java.io.IOException; import java.nio.file.FileStore; import java.nio.file.Files; @@ -37,6 +34,9 @@ import org.photonvision.common.networking.NetworkUtils; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.common.util.file.ProgramDirectoryUtilities; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.ProtobufPublisher; +import org.wpilib.vision.camera.CameraServerJNI; import oshi.SystemInfo; import oshi.hardware.CentralProcessor; import oshi.hardware.CentralProcessor.PhysicalProcessor; diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/proto/DeviceMetricsProto.java b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/proto/DeviceMetricsProto.java index b3071db93d..33947b8dfe 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/proto/DeviceMetricsProto.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/proto/DeviceMetricsProto.java @@ -17,9 +17,9 @@ package org.photonvision.common.hardware.metrics.proto; -import edu.wpi.first.util.protobuf.Protobuf; import org.photonvision.common.hardware.metrics.DeviceMetrics; import org.photonvision.proto.Photon.ProtobufDeviceMetrics; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; public class DeviceMetricsProto implements Protobuf { diff --git a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java index 8bad357969..74d4aa125b 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java @@ -17,7 +17,6 @@ package org.photonvision.common.logging; -import edu.wpi.first.math.Pair; import java.io.*; import java.nio.file.Path; import java.text.ParseException; @@ -28,6 +27,7 @@ import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.util.TimedTaskManager; +import org.wpilib.math.util.Pair; /** TODO: get rid of static {} blocks and refactor to singleton pattern */ public class Logger { diff --git a/photon-core/src/main/java/org/photonvision/common/logging/PvCSCoreLogger.java b/photon-core/src/main/java/org/photonvision/common/logging/PvCSCoreLogger.java index 9e01462602..ed5dcfd125 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/PvCSCoreLogger.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/PvCSCoreLogger.java @@ -17,8 +17,8 @@ package org.photonvision.common.logging; -import edu.wpi.first.cscore.CameraServerJNI; import java.nio.file.Path; +import org.wpilib.vision.camera.CameraServerJNI; /** Redirect cscore logs to our logger */ public class PvCSCoreLogger { diff --git a/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java b/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java index 584c4be5ee..df2525ef06 100644 --- a/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/networking/NetworkUtils.java @@ -17,7 +17,6 @@ package org.photonvision.common.networking; -import edu.wpi.first.networktables.NetworkTableInstance; import java.io.IOException; import java.net.InetAddress; import java.net.NetworkInterface; @@ -30,6 +29,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; +import org.wpilib.networktables.NetworkTableInstance; public class NetworkUtils { private static final Logger logger = new Logger(NetworkUtils.class, LogGroup.General); diff --git a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java index db41f548b1..dec2b95add 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java @@ -17,10 +17,10 @@ package org.photonvision.common.util; -import edu.wpi.first.math.geometry.Transform3d; import java.util.HashMap; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.wpilib.math.geometry.Transform3d; public final class SerializationUtils { private static final Logger logger = new Logger(SerializationUtils.class, LogGroup.General); diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java index 5c30aea889..1edbca86c2 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java @@ -18,9 +18,6 @@ package org.photonvision.common.util; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import java.awt.HeadlessException; import java.io.File; import java.io.IOException; @@ -31,6 +28,8 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.util.Units; public class TestUtils { @SuppressWarnings("unused") diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 167effaea5..d31cc0caa2 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -17,19 +17,19 @@ package org.photonvision.common.util.math; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.CoordinateSystem; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.Arrays; import java.util.List; import org.opencv.core.Core; import org.opencv.core.Mat; +import org.wpilib.math.geometry.CoordinateSystem; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.util.Units; +import org.wpilib.networktables.NetworkTablesJNI; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; public class MathUtils { MathUtils() {} diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java index 23bcb7b4e1..0d4edfbd5e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java @@ -21,8 +21,6 @@ import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.geometry.Pose3d; -import java.awt.Color; import java.nio.file.Path; import java.util.Arrays; import java.util.List; @@ -31,10 +29,7 @@ import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Point3; -import org.opencv.core.Scalar; -import org.opencv.imgcodecs.Imgcodecs; -import org.opencv.imgproc.Imgproc; -import org.photonvision.common.util.ColorHelper; +import org.wpilib.math.geometry.Pose3d; // Ignore the previous calibration data that was stored in the json file. @JsonIgnoreProperties(ignoreUnknown = true) diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java index ff463e198a..f2f640402e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java @@ -19,14 +19,14 @@ import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Num; import java.util.Arrays; import org.ejml.simple.SimpleMatrix; import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.photonvision.vision.opencv.Releasable; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.util.Num; /** JSON-serializable image. Data is stored as a raw JSON array. */ public class JsonMatOfDouble implements Releasable { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 76737fbf20..59e15ca59f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -17,9 +17,6 @@ package org.photonvision.vision.camera; -import edu.wpi.first.cscore.UsbCameraInfo; -import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.util.PixelFormat; import java.nio.file.Path; import java.util.HashMap; import org.photonvision.common.configuration.CameraConfiguration; @@ -28,6 +25,9 @@ import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.util.PixelFormat; +import org.wpilib.vision.camera.UsbCameraInfo; +import org.wpilib.vision.camera.VideoMode; public class FileVisionSource extends VisionSource { private final FileFrameProvider frameProvider; diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/PVCameraInfo.java b/photon-core/src/main/java/org/photonvision/vision/camera/PVCameraInfo.java index 96244b0787..e861bab148 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/PVCameraInfo.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/PVCameraInfo.java @@ -24,9 +24,9 @@ import com.fasterxml.jackson.annotation.JsonSubTypes; import com.fasterxml.jackson.annotation.JsonTypeInfo; import com.fasterxml.jackson.annotation.JsonTypeName; -import edu.wpi.first.cscore.UsbCameraInfo; import java.util.Arrays; import java.util.Objects; +import org.wpilib.vision.camera.UsbCameraInfo; @JsonTypeInfo(use = JsonTypeInfo.Id.NAME, include = JsonTypeInfo.As.WRAPPER_OBJECT) @JsonIgnoreProperties(ignoreUnknown = true) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311CameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311CameraSettables.java index 70af64f4bb..70f62c5429 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311CameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311CameraSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; /* * This class holds the non-windows camera quirks for the Arducam OV2311. This version supports auto-exposure, while windows does not. diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311WindowsCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311WindowsCameraSettables.java index 51cfd6ba51..31911e5944 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311WindowsCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311WindowsCameraSettables.java @@ -17,10 +17,9 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.MathUtil; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoException; /* * This class holds the windows specific camera quirks for the Arducam ov2311. A windows version is needed because windows doesn't expose the auto exposure properties of the arducam. @@ -50,7 +49,7 @@ protected void setUpExposureProperties() { public void setExposureRaw(double exposureRaw) { if (exposureRaw >= 0.0) { try { - int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure); + int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure); camera.setExposureManual(propVal); this.lastExposureRaw = exposureRaw; } catch (VideoException e) { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9281CameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9281CameraSettables.java index a2f30f64e5..3e51859e4a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9281CameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9281CameraSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; public class ArduOV9281CameraSettables extends GenericUSBCameraSettables { public ArduOV9281CameraSettables(CameraConfiguration configuration, UsbCamera camera) { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9782CameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9782CameraSettables.java index fdb3c19c36..ad90f539c1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9782CameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9782CameraSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; public class ArduOV9782CameraSettables extends GenericUSBCameraSettables { public ArduOV9782CameraSettables(CameraConfiguration configuration, UsbCamera camera) { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java index ee9932bc58..24c97a41d6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java @@ -17,12 +17,6 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.cscore.VideoProperty; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.PixelFormat; import java.util.ArrayList; import java.util.Arrays; import java.util.Collections; @@ -33,6 +27,11 @@ import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.util.PixelFormat; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoException; +import org.wpilib.vision.camera.VideoMode; +import org.wpilib.vision.camera.VideoProperty; public class GenericUSBCameraSettables extends VisionSourceSettables { // We need to remember the last exposure set when exiting @@ -131,7 +130,7 @@ public void setWhiteBalanceTemp(double tempNumber) { softSet("white_balance_automatic", 0); - int propVal = (int) MathUtil.clamp(temp, minWhiteBalanceTemp, maxWhiteBalanceTemp); + int propVal = (int) Math.clamp(temp, minWhiteBalanceTemp, maxWhiteBalanceTemp); logger.debug( "Setting property " @@ -230,7 +229,7 @@ public void setExposureRaw(double exposureRaw) { try { if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED); - int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure); + int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure); logger.debug( "Setting property " diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/InnoOV9281CameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/InnoOV9281CameraSettables.java index 3cb710e5aa..24a844c955 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/InnoOV9281CameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/InnoOV9281CameraSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; public class InnoOV9281CameraSettables extends GenericUSBCameraSettables { public InnoOV9281CameraSettables(CameraConfiguration configuration, UsbCamera camera) { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kCameraSettables.java index 40d6d19ad0..782db93a2a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kCameraSettables.java @@ -17,11 +17,10 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.MathUtil; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.util.math.MathUtils; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoException; public class LifeCam3kCameraSettables extends GenericUSBCameraSettables { // Lifecam only allows specific exposures. Pulled this list from @@ -45,7 +44,7 @@ protected void setUpExposureProperties() { public void setExposureRaw(double exposureRaw) { if (exposureRaw >= 0.0) { try { - int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure); + int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure); propVal = MathUtils.quantize(propVal, allowableExposures); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kWindowsCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kWindowsCameraSettables.java index b52dbd84de..a43c6b4e52 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kWindowsCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kWindowsCameraSettables.java @@ -17,10 +17,9 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.MathUtil; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoException; public class LifeCam3kWindowsCameraSettables extends GenericUSBCameraSettables { public LifeCam3kWindowsCameraSettables(CameraConfiguration configuration, UsbCamera camera) { @@ -41,7 +40,7 @@ protected void setUpExposureProperties() { public void setExposureRaw(double exposureRaw) { if (exposureRaw >= 0.0) { try { - int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure); + int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure); // exposureAbsProp.set(propVal); camera.setExposureManual(propVal); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/PsEyeCameraSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/PsEyeCameraSettables.java index 50e06916d0..aa66b906f6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/PsEyeCameraSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/PsEyeCameraSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; public class PsEyeCameraSettables extends GenericUSBCameraSettables { public PsEyeCameraSettables(CameraConfiguration configuration, UsbCamera camera) { diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/See3Cam24CUGSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/See3Cam24CUGSettables.java index 26f3f94d71..f0645045f3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/See3Cam24CUGSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/See3Cam24CUGSettables.java @@ -17,8 +17,8 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cscore.UsbCamera; import org.photonvision.common.configuration.CameraConfiguration; +import org.wpilib.vision.camera.UsbCamera; /* * This class holds the camera quirks for the See3Cam 24UGS. diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java index b0a4dde81a..c25b8a3781 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java @@ -17,10 +17,6 @@ package org.photonvision.vision.camera.USBCameras; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.cscore.VideoProperty; import java.util.*; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.hardware.Platform; @@ -33,6 +29,10 @@ import org.photonvision.vision.frame.provider.USBFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoException; +import org.wpilib.vision.camera.VideoProperty; +import org.wpilib.vision.stream.CameraServer; public class USBCameraSource extends VisionSource { private final Logger logger; diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java index 0ce0aa2516..3abbe04988 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java @@ -17,10 +17,6 @@ package org.photonvision.vision.camera.csi; -import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.util.PixelFormat; import java.util.HashMap; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.util.math.MathUtils; @@ -28,6 +24,9 @@ import org.photonvision.vision.camera.csi.LibcameraGpuSource.FPSRatedVideoMode; import org.photonvision.vision.opencv.ImageRotationMode; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.math.util.Pair; +import org.wpilib.util.PixelFormat; +import org.wpilib.vision.camera.VideoMode; public class LibcameraGpuSettables extends VisionSourceSettables { private FPSRatedVideoMode currentVideoMode; @@ -142,7 +141,7 @@ public void setExposureRaw(double exposureRaw) { // 80,000 uS seems like an exposure value that will be greater than ever needed while giving // enough control over exposure. - exposureRaw = MathUtil.clamp(exposureRaw, minExposure, maxExposure); + exposureRaw = Math.clamp(exposureRaw, minExposure, maxExposure); var success = LibCameraJNI.setExposure(r_ptr, (int) exposureRaw); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure"); @@ -164,7 +163,7 @@ public void setGain(int gain) { // than ever needed) from 0 to 100 (UI values). var success = LibCameraJNI.setAnalogGain( - r_ptr, MathUtil.clamp(MathUtils.map(gain, 0.0, 100.0, 1.0, 10.0), 1.0, 10.0)); + r_ptr, Math.clamp(MathUtils.map(gain, 0.0, 100.0, 1.0, 10.0), 1.0, 10.0)); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera gain"); } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSource.java index 28449e9ac2..62b1769773 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSource.java @@ -17,8 +17,6 @@ package org.photonvision.vision.camera.csi; -import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.util.PixelFormat; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; @@ -29,6 +27,8 @@ import org.photonvision.vision.frame.provider.LibcameraGpuFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.util.PixelFormat; +import org.wpilib.vision.camera.VideoMode; public class LibcameraGpuSource extends VisionSource { static final Logger logger = new Logger(LibcameraGpuSource.class, LogGroup.Camera); diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index 45af6af14d..a2d500bc44 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java @@ -17,11 +17,11 @@ package org.photonvision.vision.frame; -import edu.wpi.first.cscore.VideoMode; import org.opencv.core.Point; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.opencv.ImageRotationMode; +import org.wpilib.vision.camera.VideoMode; /** Represents the properties of a frame. */ public class FrameStaticProperties { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java index dce9366a52..56d1ab7f20 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java @@ -17,11 +17,6 @@ package org.photonvision.vision.frame.consumer; -import edu.wpi.first.networktables.IntegerEntry; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.StringSubscriber; -import edu.wpi.first.wpilibj.DriverStation.MatchType; import java.io.File; import java.text.DateFormat; import java.text.SimpleDateFormat; @@ -34,6 +29,11 @@ import org.photonvision.common.logging.Logger; import org.photonvision.vision.frame.StaticFrames; import org.photonvision.vision.opencv.CVMat; +import org.wpilib.driverstation.DriverStation.MatchType; +import org.wpilib.networktables.IntegerEntry; +import org.wpilib.networktables.IntegerSubscriber; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.StringSubscriber; public class FileSaveFrameConsumer implements Consumer { private final Logger logger = new Logger(FileSaveFrameConsumer.class, LogGroup.General); diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java index b99dc23f31..167634eec1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java @@ -17,12 +17,12 @@ package org.photonvision.vision.frame.consumer; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.*; -import edu.wpi.first.util.PixelFormat; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.StaticFrames; import org.photonvision.vision.opencv.CVMat; +import org.wpilib.util.PixelFormat; +import org.wpilib.vision.camera.*; +import org.wpilib.vision.stream.CameraServer; public class MJPGFrameConsumer implements AutoCloseable { private static final double MAX_FRAMERATE = -1; diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 85d461ca18..3a879eea11 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -17,17 +17,18 @@ package org.photonvision.vision.frame.provider; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.CvSink; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.util.PixelFormat; -import edu.wpi.first.util.RawFrame; import org.opencv.core.Mat; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.jni.CscoreExtras; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.processes.VisionSourceSettables; +import org.wpilib.networktables.BooleanSubscriber; +import org.wpilib.util.PixelFormat; +import org.wpilib.util.RawFrame; +import org.wpilib.vision.camera.CvSink; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.stream.CameraServer; public class USBFrameProvider extends CpuImageProcessor { private final Logger logger; @@ -80,7 +81,7 @@ public CapturedFrame getInputMat() { if (m_blockForFrames) { // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) var mat = new CVMat(); - // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since + // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since // Hal::initialize was called // TODO - under the hood, this incurs an extra copy. We should avoid this, if we // can. @@ -105,7 +106,7 @@ public CapturedFrame getInputMat() { cameraMode.width * 3, PixelFormat.kBGR); - // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since + // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since // Hal::initialize was called long captureTimeUs = CscoreExtras.grabRawSinkFrameTimeoutLastTime( diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java b/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java index 1aa66f94d3..9285c15a63 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java @@ -17,7 +17,6 @@ package org.photonvision.vision.opencv; -import edu.wpi.first.util.RawFrame; import java.lang.ref.PhantomReference; import java.lang.ref.ReferenceQueue; import java.util.Collections; @@ -27,6 +26,7 @@ import org.opencv.core.Mat; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.wpilib.util.RawFrame; public class CVMat implements Releasable { private static final Logger logger = new Logger(CVMat.class, LogGroup.General); diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java b/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java index 4fa3dea7e3..44ff64b456 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java @@ -17,11 +17,11 @@ package org.photonvision.vision.opencv; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import org.opencv.core.Core; import org.opencv.core.Point; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; /** * An image rotation about the camera's +Z axis, which points out of the camera towards the world. diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java index 01e237c410..b55b5696eb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java @@ -17,13 +17,13 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagDetector; import java.util.List; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; +import org.wpilib.vision.apriltag.AprilTagDetection; +import org.wpilib.vision.apriltag.AprilTagDetector; public class AprilTagDetectionPipe extends CVPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index f2c713799e..8f7932132a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -17,16 +17,16 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.apriltag.AprilTagPoseEstimator; -import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; import org.opencv.calib3d.Calib3d; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; +import org.wpilib.vision.apriltag.AprilTagDetection; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; +import org.wpilib.vision.apriltag.AprilTagPoseEstimator; +import org.wpilib.vision.apriltag.AprilTagPoseEstimator.Config; public class AprilTagPoseEstimatorPipe extends CVPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java index f036bfbfd1..5c6cc15272 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java @@ -17,11 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.ArrayList; import java.util.List; import org.opencv.calib3d.Calib3d; @@ -34,6 +29,11 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; public class ArucoPoseEstimatorPipe extends CVPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java index b840e66028..0c2af2c786 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java @@ -17,9 +17,9 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.wpilibj.Timer; import org.photonvision.vision.pipe.CVPipe; +import org.wpilib.math.filter.LinearFilter; +import org.wpilib.system.Timer; public class CalculateFPSPipe extends CVPipe { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java index ed416b1676..bbff50e556 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.awt.*; import java.util.List; import org.opencv.core.Mat; @@ -32,6 +31,7 @@ import org.photonvision.vision.target.RobotOffsetPointMode; import org.photonvision.vision.target.TargetCalculations; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; public class Draw2dCrosshairPipe extends MutatingPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java index fe14c7ca41..f919013284 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.awt.*; import java.util.List; import org.opencv.core.*; @@ -31,6 +30,7 @@ import org.photonvision.vision.opencv.ContourShape; import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; public class Draw2dTargetsPipe extends MutatingPipe>, Draw2dTargetsPipe.Draw2dTargetsParams> { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index c5183b66e7..e2251c97d7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.awt.*; import java.util.List; import org.opencv.calib3d.Calib3d; @@ -33,6 +32,7 @@ import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; public class Draw3dTargetsPipe extends MutatingPipe>, Draw3dTargetsPipe.Draw3dContoursParams> { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java index 5f6b83a177..a9e965e192 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.awt.Color; import java.util.List; import org.opencv.core.Mat; @@ -28,6 +27,7 @@ import org.photonvision.vision.frame.FrameDivisor; import org.photonvision.vision.pipe.MutatingPipe; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; public class DrawCalibrationPipe extends MutatingPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java index d6b30f2e91..8d0416bcf8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -33,6 +32,7 @@ import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.pipeline.UICalibrationData; +import org.wpilib.math.util.Pair; public class FindBoardCornersPipe extends CVPipe< diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java index 08277f0d25..7d70d83176 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.Pair; import java.util.ArrayList; import java.util.List; import org.opencv.core.Mat; @@ -27,6 +26,7 @@ import org.photonvision.vision.opencv.CVShape; import org.photonvision.vision.opencv.Contour; import org.photonvision.vision.pipe.CVPipe; +import org.wpilib.math.util.Pair; public class FindCirclesPipe extends CVPipe>, List, FindCirclesPipe.FindCirclePipeParams> { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java index b795da581f..535a7269d4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.apriltag.AprilTagFieldLayout; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -29,6 +28,7 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; /** Estimate the camera pose given multiple Apriltag observations */ public class MultiTargetPNPPipe diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java index d7af3a3585..aaecb3cffd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java @@ -17,10 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; @@ -33,6 +29,10 @@ import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.VecBuilder; public class SolvePNPPipe extends CVPipe, List, SolvePNPPipe.SolvePNPPipeParams> { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 941e16cfaa..4c3a128826 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -17,15 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagDetector; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; -import edu.wpi.first.math.geometry.CoordinateSystem; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -50,6 +41,15 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; +import org.wpilib.math.geometry.CoordinateSystem; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; +import org.wpilib.vision.apriltag.AprilTagDetection; +import org.wpilib.vision.apriltag.AprilTagDetector; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; +import org.wpilib.vision.apriltag.AprilTagPoseEstimator.Config; public class AprilTagPipeline extends CVPipeline { private static final Logger logger = new Logger(AprilTagPipeline.class, LogGroup.VisionModule); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index 49d5068252..52c2f9f5bd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -17,11 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.math.geometry.CoordinateSystem; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -45,6 +40,11 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; +import org.wpilib.math.geometry.CoordinateSystem; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; public class ArucoPipeline extends CVPipeline { private static final Logger logger = new Logger(ArucoPipeline.class, LogGroup.VisionModule); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index c1b03a2a21..e7f6a974c7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -17,8 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.util.Units; import java.nio.file.Path; import java.util.ArrayList; import java.util.List; @@ -43,6 +41,8 @@ import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.pipeline.result.CalibrationPipelineResult; +import org.wpilib.math.util.Pair; +import org.wpilib.math.util.Units; public class Calibrate3dPipeline extends CVPipeline { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java index f6fcb3459d..e73b20d7dc 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java @@ -17,9 +17,9 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.util.Units; import org.opencv.core.Size; import org.photonvision.vision.frame.FrameDivisor; +import org.wpilib.math.util.Units; public class Calibration3dPipelineSettings extends AdvancedPipelineSettings { public int boardHeight = 8; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index 0443e04576..577cef7ec4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; import java.util.Arrays; import java.util.List; import org.opencv.core.Point; @@ -32,6 +31,7 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; public class ColoredShapePipeline extends CVPipeline { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index a8c52efc85..c4caad708f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; import java.util.List; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; @@ -26,6 +25,7 @@ import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; import org.photonvision.vision.pipe.impl.ResizeImagePipe; import org.photonvision.vision.pipeline.result.DriverModePipelineResult; +import org.wpilib.math.util.Pair; public class DriverModePipeline extends CVPipeline { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index 9d8006ec91..74d46d7b63 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; import java.util.List; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; @@ -25,6 +24,7 @@ import org.photonvision.vision.pipe.impl.*; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Pair; /** * This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 51773c9f16..2db7fcbab9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -104,9 +104,9 @@ public void release() { } /** - * Get the latency between now (wpi::Now) and the time at which the image was captured. FOOTGUN: - * the latency is relative to the time at which this method is called. Waiting to call this method - * will change the latency this method returns. + * Get the latency between now (wpi::util::Now) and the time at which the image was captured. + * FOOTGUN: the latency is relative to the time at which this method is called. Waiting to call + * this method will change the latency this method returns. */ @Deprecated public double getLatencyMillis() { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index b57b01d969..f1e38a8502 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -17,9 +17,6 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.util.Units; import io.javalin.websocket.WsContext; import java.util.ArrayList; import java.util.HashMap; @@ -57,6 +54,9 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.util.Units; +import org.wpilib.vision.camera.CameraServerJNI; +import org.wpilib.vision.camera.VideoException; /** * This is the God Class diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index ffdd27fc6c..c98c128d1f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -18,7 +18,6 @@ package org.photonvision.vision.processes; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.Pair; import java.util.ArrayList; import java.util.LinkedHashMap; import java.util.List; @@ -39,6 +38,7 @@ import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.pipeline.UICalibrationData; import org.photonvision.vision.target.RobotOffsetPointOperation; +import org.wpilib.math.util.Pair; @SuppressWarnings("unchecked") public class VisionModuleChangeSubscriber extends DataChangeSubscriber { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index b46e4d1a44..22d1f43f2e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -17,7 +17,6 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.UsbCamera; import java.util.ArrayList; import java.util.Arrays; import java.util.Collection; @@ -47,6 +46,7 @@ import org.photonvision.vision.camera.PVCameraInfo; import org.photonvision.vision.camera.USBCameras.USBCameraSource; import org.photonvision.vision.camera.csi.LibcameraGpuSource; +import org.wpilib.vision.camera.UsbCamera; /** * This class manages starting up VisionModules for serialized devices ({@link diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 331ea10b8b..e22dc12009 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -17,7 +17,6 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.VideoMode; import java.util.HashMap; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; @@ -25,6 +24,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.FrameStaticProperties; +import org.wpilib.vision.camera.VideoMode; public abstract class VisionSourceSettables { protected Logger logger; diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java index 65b7ec22c6..b53ea6ff9d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -21,7 +21,6 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; import org.opencv.core.MatOfPoint3f; @@ -29,6 +28,7 @@ import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipe.impl.CornerDetectionPipe; import org.photonvision.vision.pipe.impl.SolvePNPPipe; +import org.wpilib.math.util.Units; /** * A model representing the vertices of targets with known shapes. The vertices are in the EDN diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 1c5e22c05d..1160651d2b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -17,9 +17,6 @@ package org.photonvision.vision.target; -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.math.geometry.Transform3d; import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -40,6 +37,9 @@ import org.photonvision.vision.opencv.Contour; import org.photonvision.vision.opencv.DualOffsetValues; import org.photonvision.vision.opencv.Releasable; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.vision.apriltag.AprilTagDetection; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; public class TrackedTarget implements Releasable { public final Contour m_mainContour; diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java index 4bc11bbf75..4995cbceec 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java @@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import com.fasterxml.jackson.core.JsonProcessingException; -import edu.wpi.first.cscore.UsbCameraInfo; import java.io.IOException; import java.nio.file.Path; import java.util.Collection; @@ -43,6 +42,7 @@ import org.photonvision.vision.pipeline.ObjectDetectionPipelineSettings; import org.photonvision.vision.pipeline.PipelineType; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; +import org.wpilib.vision.camera.UsbCameraInfo; public class SQLConfigTest { @TempDir private Path tmpDir; diff --git a/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java index ae70fca0e9..37ee6a55c6 100644 --- a/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java +++ b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java @@ -19,14 +19,14 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.common.LoadJNI; import org.photonvision.common.util.math.MathUtils; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.VecBuilder; public class CoordinateConversionTest { @BeforeAll diff --git a/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java b/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java index 0595d8b2f5..5c2f596033 100644 --- a/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java @@ -22,13 +22,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.MatchType; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.simulation.SimHooks; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.File; import java.io.IOException; import java.util.Date; @@ -43,6 +36,13 @@ import org.photonvision.common.util.TestUtils; import org.photonvision.jni.LibraryLoader; import org.photonvision.vision.frame.provider.FileFrameProvider; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.DriverStation.MatchType; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.simulation.DriverStationSim; +import org.wpilib.simulation.SimHooks; +import org.wpilib.smartdashboard.SmartDashboard; public class FileSaveFrameConsumerTest { NetworkTableInstance inst = null; diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java index c69ffad1dd..fc808eb00f 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java @@ -19,7 +19,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Translation3d; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.photonvision.common.LoadJNI; @@ -30,6 +29,7 @@ import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; +import org.wpilib.math.geometry.Translation3d; public class AprilTagTest { @BeforeEach diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java index 6bbb149eb6..6a6abd25d4 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java @@ -19,7 +19,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Translation3d; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.photonvision.common.LoadJNI; @@ -30,6 +29,7 @@ import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; +import org.wpilib.math.geometry.Translation3d; public class ArucoPipelineTest { @BeforeEach diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index ccb5e0c027..fac991ff3e 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -20,7 +20,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.util.Units; import java.io.File; import java.io.IOException; import java.nio.file.Path; @@ -47,6 +46,7 @@ import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.pipeline.UICalibrationData.BoardType; import org.photonvision.vision.pipeline.UICalibrationData.TagFamily; +import org.wpilib.math.util.Units; public class Calibrate3dPipeTest { @BeforeAll diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index df3c6daa95..0d8b7f9863 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -21,9 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.photonvision.common.LoadJNI; @@ -38,6 +35,9 @@ import org.photonvision.vision.pipe.impl.HSVPipe; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; public class SolvePNPTest { private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json"; diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/MockUsbCameraSource.java b/photon-core/src/test/java/org/photonvision/vision/processes/MockUsbCameraSource.java index e17367adc2..8bf976787b 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/MockUsbCameraSource.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/MockUsbCameraSource.java @@ -17,14 +17,14 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoMode; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.camera.USBCameras.GenericUSBCameraSettables; import org.photonvision.vision.camera.USBCameras.USBCameraSource; import org.photonvision.vision.frame.provider.FileFrameProvider; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.camera.VideoMode; public class MockUsbCameraSource extends USBCameraSource { /** Used for unit tests to better simulate a usb camera without a camera being present. */ diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index ba1025fb1b..63add51bde 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.cscore.VideoMode; import java.util.Arrays; import java.util.HashMap; import java.util.List; @@ -40,6 +39,7 @@ import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.wpilib.vision.camera.VideoMode; public class VisionModuleManagerTest { @BeforeAll diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index 0673457870..83114e3208 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.cscore.UsbCameraInfo; import java.io.IOException; import java.util.ArrayList; import java.util.List; @@ -35,6 +34,7 @@ import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.vision.camera.PVCameraInfo; +import org.wpilib.vision.camera.UsbCameraInfo; public class VisionSourceManagerTest { // Test harness that overrides getConnectedCameras, but uses USB cameras for diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index 5089e4e79c..c8b314be87 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -20,8 +20,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import java.util.stream.Stream; import org.junit.jupiter.api.BeforeAll; @@ -39,6 +37,8 @@ import org.photonvision.vision.calibration.JsonMatOfDouble; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.DualOffsetValues; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; public class TargetCalculationsTest { private static Size imageSize = new Size(1280, 720); diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 5703c9081d..ce0e4a97b0 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -12,7 +12,7 @@ ext { apply plugin: 'cpp' apply plugin: 'google-test-test-suite' -apply plugin: 'edu.wpi.first.NativeUtils' +apply plugin: 'org.wpilib.NativeUtils' apply from: "${rootDir}/shared/config.gradle" apply from: "${rootDir}/shared/javacommon.gradle" @@ -93,7 +93,7 @@ model { nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "cscore_shared") nativeUtils.useRequiredLibrary(it, "cameraserver_shared") - nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") + nativeUtils.useRequiredLibrary(it, "wpilib_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") nativeUtils.useRequiredLibrary(it, "opencv_shared") diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index faffe998fb..728e6e96d4 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -24,10 +24,10 @@ package org.photonvision; -import edu.wpi.first.math.geometry.Pose3d; import java.util.List; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.math.geometry.Pose3d; /** An estimated pose based on pipeline result */ public class EstimatedRobotPose { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 0d4cb7f179..61467f6cb3 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,26 +24,6 @@ package org.photonvision; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.networktables.BooleanPublisher; -import edu.wpi.first.networktables.BooleanSubscriber; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.IntegerEntry; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.MultiSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StringSubscriber; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -53,6 +33,26 @@ import org.photonvision.common.networktables.PacketSubscriber; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.timesync.TimeSyncSingleton; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.numbers.*; +import org.wpilib.math.util.Nat; +import org.wpilib.networktables.BooleanPublisher; +import org.wpilib.networktables.BooleanSubscriber; +import org.wpilib.networktables.DoubleArraySubscriber; +import org.wpilib.networktables.IntegerEntry; +import org.wpilib.networktables.IntegerPublisher; +import org.wpilib.networktables.IntegerSubscriber; +import org.wpilib.networktables.MultiSubscriber; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.PubSubOption; +import org.wpilib.networktables.StringSubscriber; +import org.wpilib.system.Timer; +import org.wpilib.util.Alert; +import org.wpilib.util.Alert.AlertType; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index f7f0c2c1dc..0db4785f48 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -24,28 +24,28 @@ package org.photonvision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import edu.wpi.first.wpilibj.DriverStation; import java.util.*; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.interpolation.TimeInterpolatableBuffer; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.numbers.N8; +import org.wpilib.math.util.Pair; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.camera.OpenCvLoader; /** * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a diff --git a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java index e9ea78c9ee..3b1003643b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java @@ -24,7 +24,7 @@ package org.photonvision; -import edu.wpi.first.math.geometry.*; +import org.wpilib.math.geometry.*; public final class PhotonUtils { private PhotonUtils() { @@ -39,7 +39,7 @@ private PhotonUtils() { * for there to exist a height differential between goal and camera. The larger this differential, * the more accurate the distance estimate will be. * - *

Units can be converted using the {@link edu.wpi.first.math.util.Units} class. + *

Units can be converted using the {@link org.wpilib.math.util.Units} class. * * @param cameraHeightMeters The physical height of the camera off the floor in meters. * @param targetHeightMeters The physical height of the target off the floor in meters. This diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 68d3023285..af9ff94de5 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -24,19 +24,6 @@ package org.photonvision.simulation; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.CvSource; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.PixelFormat; -import edu.wpi.first.util.WPIUtilJNI; -import edu.wpi.first.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -60,6 +47,18 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PnpResult; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Pair; +import org.wpilib.system.RobotController; +import org.wpilib.util.PixelFormat; +import org.wpilib.util.WPIUtilJNI; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; +import org.wpilib.vision.camera.CvSource; +import org.wpilib.vision.camera.OpenCvLoader; +import org.wpilib.vision.camera.VideoSource.ConnectionStrategy; +import org.wpilib.vision.stream.CameraServer; /** * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this @@ -295,8 +294,8 @@ public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) { */ public boolean canSeeCorners(Point[] points) { for (var point : points) { - if (MathUtil.clamp(point.x, 0, prop.getResWidth()) != point.x - || MathUtil.clamp(point.y, 0, prop.getResHeight()) != point.y) { + if (Math.clamp(point.x, 0, prop.getResWidth()) != point.x + || Math.clamp(point.y, 0, prop.getResHeight()) != point.y) { return false; // point is outside of resolution } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 620a076f71..7f602f6f5b 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,19 +25,6 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; -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.Pair; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.wpilibj.DriverStation; import java.io.IOException; import java.nio.file.Path; import java.util.ArrayList; @@ -51,6 +38,18 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.RotTrlTransform3d; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.linalg.Vector; +import org.wpilib.math.numbers.*; +import org.wpilib.math.util.Nat; +import org.wpilib.math.util.Pair; /** * Calibration and performance values for this camera. @@ -167,7 +166,7 @@ public SimCameraProperties setRandomSeed(long seed) { public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) { - fovDiag = Rotation2d.fromDegrees(MathUtil.clamp(fovDiag.getDegrees(), 1, 179)); + fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179)); DriverStation.reportError( "Requested invalid FOV! Clamping between (1, 179) degrees...", false); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index b3305581ce..f6238ae7bb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -24,13 +24,6 @@ package org.photonvision.simulation; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.cscore.CvSource; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.RawFrame; import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; @@ -48,6 +41,13 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.RotTrlTransform3d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; +import org.wpilib.util.RawFrame; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.camera.CvSource; +import org.wpilib.vision.camera.OpenCvLoader; public class VideoSimUtil { // Tag IDs start at 0, this should be set to 1 greater than the maximum tag ID required diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 29b79a8f0d..a2dca15cfd 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -24,15 +24,6 @@ package org.photonvision.simulation; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.Collection; import java.util.HashMap; @@ -43,6 +34,15 @@ import java.util.Set; import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.interpolation.TimeInterpolatableBuffer; +import org.wpilib.smartdashboard.Field2d; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.Timer; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; /** * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java index 489bbdff1f..2ecf57284c 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java @@ -24,10 +24,10 @@ package org.photonvision.simulation; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.photonvision.estimation.TargetModel; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Translation3d; /** Describes a vision target located somewhere on the field that your vision system can detect. */ public class VisionTargetSim { diff --git a/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java index bf64412f79..d8c3eccb4a 100644 --- a/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java +++ b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java @@ -24,9 +24,9 @@ package org.photonvision.timesync; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import org.photonvision.jni.TimeSyncServer; +import org.wpilib.util.runtime.RuntimeLoader; /** Helper to hold a single TimeSyncServer instance with some default config */ public class TimeSyncSingleton { diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index bb00064788..abcb24169c 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -29,20 +29,21 @@ #include #include -#include -#include -#include -#include +#include #include #include #include -#include +#include +#include +#include +#include +#include #include "PhotonVersion.h" #include "photon/dataflow/structures/Packet.h" -static constexpr units::second_t WARN_DEBOUNCE_SEC = 5_s; -static constexpr units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms; +static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s; +static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms; inline void verifyDependencies() { if (!(std::string_view{cv::getVersionString()} == @@ -83,8 +84,8 @@ inline void verifyDependencies() { ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n" ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"; - FRC_ReportWarning(bfw); - FRC_ReportError(frc::err::Error, bfw); + WPILIB_ReportWarning(bfw); + WPILIB_ReportError(wpi::err::Error, bfw); throw new std::runtime_error(std::string{bfw}); } } @@ -107,7 +108,7 @@ static void InitTspServer() { namespace photon { -constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s; +constexpr const wpi::units::second_t VERSION_CHECK_INTERVAL = 5_s; static const std::vector PHOTON_PREFIX = {"/photonvision/"}; static const std::string PHOTON_ALERT_GROUP{"PhotonAlerts"}; bool PhotonCamera::VERSION_CHECK_ENABLED = true; @@ -120,7 +121,7 @@ static const std::string TYPE_STRING = std::string{"photonstruct:PhotonPipelineResult:"} + std::string{SerdeType::GetSchemaHash()}; -PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, +PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), @@ -163,20 +164,20 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, disconnectAlert(PHOTON_ALERT_GROUP, std::string{"PhotonCamera '"} + std::string{cameraName} + "' is disconnected.", - frc::Alert::AlertType::kWarning), - timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) { + wpi::Alert::AlertType::kWarning), + timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::AlertType::kWarning) { verifyDependencies(); InstanceCount++; HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, ""); // The Robot class is actually created here: - // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33 + // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/wpi/opmode/RobotBase.hpp#L33 // so we should be fine to call this from the ctor InitTspServer(); } PhotonCamera::PhotonCamera(const std::string_view cameraName) - : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} + : PhotonCamera(wpi::nt::NetworkTableInstance::GetDefault(), cameraName) {} PhotonPipelineResult PhotonCamera::GetLatestResult() { if (test) { @@ -190,8 +191,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { VerifyVersion(); // Fill the packet with latest data and populate result. - units::microsecond_t now = - units::microsecond_t(frc::RobotController::GetFPGATime()); + wpi::units::microsecond_t now = + wpi::units::microsecond_t(wpi::RobotController::GetFPGATime()); const auto value = rawBytesEntry.Get(); if (!value.size()) return PhotonPipelineResult{}; @@ -223,7 +224,7 @@ std::vector PhotonCamera::GetAllUnreadResults() { ret.reserve(changes.size()); for (size_t i = 0; i < changes.size(); i++) { - const nt::Timestamped>& value = changes[i]; + const wpi::nt::Timestamped>& value = changes[i]; if (!value.value.size() || value.time == 0) { continue; @@ -237,7 +238,7 @@ std::vector PhotonCamera::GetAllUnreadResults() { // TODO: NT4 timestamps are still not to be trusted. But it's the best we // can do until we can make time sync more reliable. - result.SetReceiveTimestamp(units::microsecond_t(value.time) - + result.SetReceiveTimestamp(wpi::units::microsecond_t(value.time) - result.GetLatency()); ret.push_back(result); @@ -261,11 +262,11 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) { timesyncAlert.SetText(warningText); timesyncAlert.Set(true); - if (frc::Timer::GetFPGATimestamp() < + if (wpi::Timer::GetFPGATimestamp() < (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { - prevTimeSyncWarnTime = frc::Timer::GetFPGATimestamp(); + prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp(); - FRC_ReportWarning( + WPILIB_ReportWarning( warningText + "\n\nCheck /photonvision/.timesync/{{COPROCESSOR_HOSTNAME}} for more " "information."); @@ -317,7 +318,7 @@ const std::string_view PhotonCamera::GetCameraName() const { bool PhotonCamera::IsConnected() { auto currentHeartbeat = heartbeatSubscriber.Get(); - auto now = frc::Timer::GetFPGATimestamp(); + auto now = wpi::Timer::GetFPGATimestamp(); if (currentHeartbeat < 0) { // we have never heard from the camera @@ -365,10 +366,10 @@ void PhotonCamera::VerifyVersion() { return; } - if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) < + if ((wpi::Timer::GetFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; - this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp(); + this->lastVersionCheckTime = wpi::Timer::GetFPGATimestamp(); const std::string& versionString = versionEntry.Get(""); if (versionString.empty()) { @@ -376,13 +377,13 @@ void PhotonCamera::VerifyVersion() { std::vector cameraNames = rootTable->GetInstance().GetTable("photonvision")->GetSubTables(); if (cameraNames.empty()) { - FRC_ReportError(frc::warn::Warning, - "Could not find any PhotonVision coprocessors on " - "NetworkTables. Double check that PhotonVision is " - "running, and that your camera is connected!"); + WPILIB_ReportError(wpi::warn::Warning, + "Could not find any PhotonVision coprocessors on " + "NetworkTables. Double check that PhotonVision is " + "running, and that your camera is connected!"); } else { - FRC_ReportError( - frc::warn::Warning, + WPILIB_ReportError( + wpi::warn::Warning, "PhotonVision coprocessor at path {} not found on NetworkTables. " "Double check that your camera names match!", path_); @@ -391,8 +392,8 @@ void PhotonCamera::VerifyVersion() { for (unsigned int i = 0; i < cameraNames.size(); i++) { cameraNameOutString += ("\n" + cameraNames[i]); } - FRC_ReportError( - frc::warn::Warning, + WPILIB_ReportError( + wpi::warn::Warning, "Found the following PhotonVision cameras on NetworkTables:\n{}", cameraNameOutString); } @@ -400,12 +401,12 @@ void PhotonCamera::VerifyVersion() { std::string local_uuid{SerdeType::GetSchemaHash()}; // implicit conversion here might throw an exception, so be careful of that - wpi::json remote_uuid_json = + wpi::util::json remote_uuid_json = rawBytesEntry.GetTopic().GetProperty("message_uuid"); if (!remote_uuid_json.is_string()) { - FRC_ReportError(frc::warn::Warning, - "Cannot find property message_uuid for PhotonCamera {}", - path); + WPILIB_ReportError( + wpi::warn::Warning, + "Cannot find property message_uuid for PhotonCamera {}", path); return; } std::string remote_uuid{remote_uuid_json}; @@ -429,12 +430,12 @@ void PhotonCamera::VerifyVersion() { ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n" ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" "\n\n"; - FRC_ReportWarning(bfw); + WPILIB_ReportWarning(bfw); std::string error_str = fmt::format( "Photonlib version {} (message definition version {}) does not match " "coprocessor version {} (message definition version {})!", PhotonVersion::versionString, local_uuid, versionString, remote_uuid); - FRC_ReportError(frc::err::Error, "{}", error_str); + WPILIB_ReportError(wpi::err::Error, "{}", error_str); throw std::runtime_error(error_str); } } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index a720898093..3dd32a58e0 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -30,18 +30,18 @@ #include #include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include "photon/PhotonCamera.h" #include "photon/estimation/TargetModel.h" @@ -55,37 +55,41 @@ WPI_IGNORE_DEPRECATED namespace photon { namespace detail { -cv::Point3d ToPoint3d(const frc::Translation3d& translation); +cv::Point3d ToPoint3d(const wpi::math::Translation3d& translation); std::optional> CalcTagCorners( - int tagID, const frc::AprilTagFieldLayout& aprilTags); -frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec); -cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, - units::meter_t cornerY, frc::Pose3d tagPose); + int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags); +wpi::math::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec); +cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX, + wpi::units::meter_t cornerY, + wpi::math::Pose3d tagPose); } // namespace detail -PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, - frc::Transform3d robotToCamera) +PhotonPoseEstimator::PhotonPoseEstimator( + wpi::apriltag::AprilTagFieldLayout tags, + wpi::math::Transform3d robotToCamera) : aprilTags(tags), m_robotToCamera(robotToCamera), - lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()), + lastPose(wpi::math::Pose3d()), + referencePose(wpi::math::Pose3d()), poseCacheTimestamp(-1_s), - headingBuffer(frc::TimeInterpolatableBuffer(1_s)) { + headingBuffer( + wpi::math::TimeInterpolatableBuffer(1_s)) { HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, InstanceCount); InstanceCount++; } -PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, - PoseStrategy strat, - frc::Transform3d robotToCamera) +PhotonPoseEstimator::PhotonPoseEstimator( + wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat, + wpi::math::Transform3d robotToCamera) : aprilTags(tags), strategy(strat), m_robotToCamera(robotToCamera), - lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()), + lastPose(wpi::math::Pose3d()), + referencePose(wpi::math::Pose3d()), poseCacheTimestamp(-1_s), - headingBuffer(frc::TimeInterpolatableBuffer(1_s)) { + headingBuffer( + wpi::math::TimeInterpolatableBuffer(1_s)) { InstanceCount++; HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); } @@ -93,8 +97,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || strategy == MULTI_TAG_PNP_ON_RIO) { - FRC_ReportError( - frc::warn::Warning, + WPILIB_ReportError( + wpi::warn::Warning, "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", ""); strategy = LOWEST_AMBIGUITY; @@ -112,15 +116,16 @@ std::optional PhotonPoseEstimator::Update( std::optional constrainedPnpParams) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { - FRC_ReportError(frc::warn::Warning, - "Result timestamp was reported in the past!"); + WPILIB_ReportError(wpi::warn::Warning, + "Result timestamp was reported in the past!"); return std::nullopt; } // If the pose cache timestamp was set, and the result is from the same // timestamp, return an empty result if (poseCacheTimestamp > 0_s && - units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) { + wpi::units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < + 0.001_ms) { return std::nullopt; } @@ -170,9 +175,9 @@ std::optional PhotonPoseEstimator::Update( break; case MULTI_TAG_PNP_ON_RIO: if (!cameraMatrixData && !cameraDistCoeffs) { - FRC_ReportError(frc::warn::Warning, - "No camera calibration provided to multi-tag-on-rio!", - ""); + WPILIB_ReportError( + wpi::warn::Warning, + "No camera calibration provided to multi-tag-on-rio!", ""); ret = Update(result, this->multiTagFallbackStrategy); } ret = @@ -185,8 +190,8 @@ std::optional PhotonPoseEstimator::Update( using namespace frc; if (!cameraMatrixData || !cameraDistCoeffs) { - FRC_ReportError( - frc::warn::Warning, + WPILib_ReportError( + wpi::warn::Warning, "No camera calibration data provided for Constrained SolvePnP!"); ret = Update(result, this->multiTagFallbackStrategy); break; @@ -203,12 +208,12 @@ std::optional PhotonPoseEstimator::Update( break; } - frc::Pose3d fieldToRobotSeed; + wpi::math::Pose3d fieldToRobotSeed; if (result.MultiTagResult().has_value()) { fieldToRobotSeed = - frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best + - m_robotToCamera.Inverse()); + wpi::math::Pose3d{} + (result.MultiTagResult()->estimatedPose.best + + m_robotToCamera.Inverse()); } else { std::optional nestedUpdate = Update(result, cameraMatrixData, cameraDistCoeffs, {}, @@ -236,8 +241,8 @@ std::optional PhotonPoseEstimator::Update( ret = EstimatePnpDistanceTrigSolvePose(result); break; default: - FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", - ""); + WPILIB_ReportError(wpi::warn::Warning, "Invalid Pose Strategy selected!", + ""); ret = std::nullopt; } @@ -250,8 +255,8 @@ std::optional PhotonPoseEstimator::Update( bool ShouldEstimate(const PhotonPipelineResult& result) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { - FRC_ReportError(frc::warn::Warning, - "Result timestamp was reported in the past!"); + WPILib_ReportError(wpi::warn::Warning, + "Result timestamp was reported in the past!"); return false; } @@ -281,12 +286,12 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose( auto& bestTarget = *foundIt; - std::optional fiducialPose = + std::optional fiducialPose = aprilTags.GetTagPose(bestTarget.GetFiducialId()); if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - bestTarget.GetFiducialId()); + WPILIB_ReportError(wpi::warn::Warning, + "Tried to get pose of unknown April Tag: {}", + bestTarget.GetFiducialId()); return std::nullopt; } @@ -302,28 +307,28 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( if (!ShouldEstimate(cameraResult)) { return std::nullopt; } - units::meter_t smallestHeightDifference = - units::meter_t(std::numeric_limits::infinity()); + wpi::units::meter_t smallestHeightDifference = + wpi::units::meter_t(std::numeric_limits::infinity()); std::optional pose = std::nullopt; for (auto& target : cameraResult.GetTargets()) { - std::optional fiducialPose = + std::optional fiducialPose = aprilTags.GetTagPose(target.GetFiducialId()); if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); + WPILIB_ReportError(wpi::warn::Warning, + "Tried to get pose of unknown April Tag: {}", + target.GetFiducialId()); continue; } - frc::Pose3d const targetPose = *fiducialPose; + wpi::math::Pose3d const targetPose = *fiducialPose; - units::meter_t const alternativeDifference = units::math::abs( + wpi::units::meter_t const alternativeDifference = wpi::units::math::abs( m_robotToCamera.Z() - targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) .Z()); - units::meter_t const bestDifference = units::math::abs( + wpi::units::meter_t const bestDifference = wpi::units::math::abs( m_robotToCamera.Z() - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z()); @@ -350,26 +355,26 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( std::optional PhotonPoseEstimator::EstimateClosestToReferencePose( - PhotonPipelineResult cameraResult, frc::Pose3d referencePose) { + PhotonPipelineResult cameraResult, wpi::math::Pose3d referencePose) { if (!ShouldEstimate(cameraResult)) { return std::nullopt; } - units::meter_t smallestDifference = - units::meter_t(std::numeric_limits::infinity()); - units::second_t stateTimestamp = units::second_t(0); - frc::Pose3d pose = lastPose; + wpi::units::meter_t smallestDifference = + wpi::units::meter_t(std::numeric_limits::infinity()); + wpi::units::second_t stateTimestamp = wpi::units::second_t(0); + wpi::math::Pose3d pose = lastPose; auto targets = cameraResult.GetTargets(); for (auto& target : targets) { - std::optional fiducialPose = + std::optional fiducialPose = aprilTags.GetTagPose(target.GetFiducialId()); if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); + WPILIB_ReportError(wpi::warn::Warning, + "Tried to get pose of unknown April Tag: {}", + target.GetFiducialId()); continue; } - frc::Pose3d targetPose = fiducialPose.value(); + wpi::math::Pose3d targetPose = fiducialPose.value(); const auto altPose = targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) @@ -378,9 +383,9 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()); - units::meter_t const alternativeDifference = units::math::abs( + wpi::units::meter_t const alternativeDifference = wpi::units::math::abs( referencePose.Translation().Distance(altPose.Translation())); - units::meter_t const bestDifference = units::math::abs( + wpi::units::meter_t const bestDifference = wpi::units::math::abs( referencePose.Translation().Distance(bestPose.Translation())); if (alternativeDifference < smallestDifference) { smallestDifference = alternativeDifference; @@ -400,7 +405,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( } std::optional> detail::CalcTagCorners( - int tagID, const frc::AprilTagFieldLayout& aprilTags) { + int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags) { if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) { return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose), TagCornerToObjectPoint(+3_in, -3_in, *tagPose), @@ -411,23 +416,23 @@ std::optional> detail::CalcTagCorners( } } -cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) { +cv::Point3d detail::ToPoint3d(const wpi::math::Translation3d& translation) { return cv::Point3d(-translation.Y().value(), -translation.Z().value(), +translation.X().value()); } -cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX, - units::meter_t cornerY, - frc::Pose3d tagPose) { - frc::Translation3d cornerTrans = - tagPose.Translation() + - frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation()); +cv::Point3d detail::TagCornerToObjectPoint(wpi::units::meter_t cornerX, + wpi::units::meter_t cornerY, + wpi::math::Pose3d tagPose) { + wpi::math::Translation3d cornerTrans = + tagPose.Translation() + wpi::math::Translation3d(0.0_m, cornerX, cornerY) + .RotateBy(tagPose.Rotation()); return ToPoint3d(cornerTrans); } -frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { - using namespace frc; - using namespace units; +wpi::math::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { + using namespace wpi::math; + using namespace wpi::units; cv::Mat R; cv::Rodrigues(rvec, R); // R is 3x3 @@ -458,7 +463,7 @@ PhotonPoseEstimator::EstimateCoprocMultiTagPose( const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best; const auto fieldToRobot = - frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); + wpi::math::Pose3d() + field2camera + m_robotToCamera.Inverse(); return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(), cameraResult.GetTargets(), MULTI_TAG_PNP_ON_COPROCESSOR); @@ -512,7 +517,7 @@ std::optional PhotonPoseEstimator::EstimateRioMultiTagPose( tvec, false, cv::SOLVEPNP_SQPNP); } - const frc::Pose3d pose = detail::ToPose3d(tvec, rvec); + const wpi::math::Pose3d pose = detail::ToPose3d(tvec, rvec); return photon::EstimatedRobotPose( pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(), @@ -526,47 +531,50 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose( return std::nullopt; } PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget(); - std::optional headingSampleOpt = + std::optional headingSampleOpt = headingBuffer.Sample(cameraResult.GetTimestamp()); if (!headingSampleOpt) { - FRC_ReportError(frc::warn::Warning, - "There was no heading data! Use AddHeadingData to add it!"); + WPILIB_ReportError( + wpi::warn::Warning, + "There was no heading data! Use AddHeadingData to add it!"); return std::nullopt; } - frc::Rotation2d headingSample = headingSampleOpt.value(); + wpi::math::Rotation2d headingSample = headingSampleOpt.value(); - frc::Translation2d camToTagTranslation = - frc::Translation3d( + wpi::math::Translation2d camToTagTranslation = + wpi::math::Translation3d( bestTarget.GetBestCameraToTarget().Translation().Norm(), - frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()), - -units::degree_t(bestTarget.GetYaw()))) + wpi::math::Rotation3d(0_rad, + -wpi::units::degree_t(bestTarget.GetPitch()), + -wpi::units::degree_t(bestTarget.GetYaw()))) .RotateBy(m_robotToCamera.Rotation()) .ToTranslation2d() .RotateBy(headingSample); - std::optional fiducialPose = + std::optional fiducialPose = aprilTags.GetTagPose(bestTarget.GetFiducialId()); if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - bestTarget.GetFiducialId()); + WPILIB_ReportError(wpi::warn::Warning, + "Tried to get pose of unknown April Tag: {}", + bestTarget.GetFiducialId()); return std::nullopt; } - frc::Pose2d tagPose = fiducialPose.value().ToPose2d(); + wpi::math::Pose2d tagPose = fiducialPose.value().ToPose2d(); - frc::Translation2d fieldToCameraTranslation = + wpi::math::Translation2d fieldToCameraTranslation = tagPose.Translation() - camToTagTranslation; - frc::Translation2d camToRobotTranslation = + wpi::math::Translation2d camToRobotTranslation = (-m_robotToCamera.Translation().ToTranslation2d()) .RotateBy(headingSample); - frc::Pose2d robotPose = frc::Pose2d( + wpi::math::Pose2d robotPose = wpi::math::Pose2d( fieldToCameraTranslation + camToRobotTranslation, headingSample); - return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(), + return EstimatedRobotPose{wpi::math::Pose3d(robotPose), + cameraResult.GetTimestamp(), cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE}; } @@ -576,22 +584,23 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( if (!ShouldEstimate(cameraResult)) { return std::nullopt; } - std::vector>> + std::vector< + std::pair>> tempPoses; double totalAmbiguity = 0; auto targets = cameraResult.GetTargets(); for (auto& target : targets) { - std::optional fiducialPose = + std::optional fiducialPose = aprilTags.GetTagPose(target.GetFiducialId()); if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); + WPILIB_ReportError(wpi::warn::Warning, + "Tried to get pose of unknown April Tag: {}", + target.GetFiducialId()); continue; } - frc::Pose3d targetPose = fiducialPose.value(); + wpi::math::Pose3d targetPose = fiducialPose.value(); // Ambiguity = 0, use that pose if (target.GetPoseAmbiguity() == 0) { return EstimatedRobotPose{ @@ -608,17 +617,17 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( cameraResult.GetTimestamp()))); } - frc::Translation3d transform = frc::Translation3d(); - frc::Rotation3d rotation = frc::Rotation3d(); + wpi::math::Translation3d transform = wpi::math::Translation3d(); + wpi::math::Rotation3d rotation = wpi::math::Rotation3d(); - for (std::pair>& pair : - tempPoses) { + for (std::pair>& + pair : tempPoses) { double const weight = (1. / pair.second.first) / totalAmbiguity; transform = transform + pair.first.Translation() * weight; rotation = rotation + pair.first.Rotation() * weight; } - return EstimatedRobotPose{frc::Pose3d(transform, rotation), + return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation), cameraResult.GetTimestamp(), cameraResult.GetTargets(), AVERAGE_BEST_TARGETS}; } @@ -627,8 +636,8 @@ std::optional PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( photon::PhotonPipelineResult cameraResult, photon::PhotonCamera::CameraMatrix cameraMatrix, - photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose, - bool headingFree, double headingScaleFactor) { + photon::PhotonCamera::DistortionMatrix distCoeffs, + wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor) { if (!ShouldEstimate(cameraResult)) { return std::nullopt; } @@ -638,9 +647,9 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( return std::nullopt; } else { // If heading fixed, force rotation component - seedPose = frc::Pose3d{ + seedPose = wpi::math::Pose3d{ seedPose.Translation(), - frc::Rotation3d{ + wpi::math::Rotation3d{ headingBuffer.Sample(cameraResult.GetTimestamp()).value()}}; } } @@ -651,7 +660,8 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( VisionEstimation::EstimateRobotPoseConstrainedSolvePNP( cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose, aprilTags, photon::kAprilTag36h11, headingFree, - frc::Rotation2d{ + wpi::math::Rotation2d{ + headingBuffer.Sample(cameraResult.GetTimestamp()).value()}, headingScaleFactor); @@ -659,7 +669,7 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( return std::nullopt; } - frc::Pose3d best = frc::Pose3d{} + pnpResult->best; + wpi::math::Pose3d best = wpi::math::Pose3d{} + pnpResult->best; return EstimatedRobotPose{best, cameraResult.GetTimestamp(), cameraResult.GetTargets(), diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 4d3660a057..e3e289eb6f 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -29,8 +29,8 @@ #include #include -#include -#include +#include +#include #include "photon/estimation/CameraTargetRelation.h" #include "photon/estimation/RotTrlTransform3d.h" @@ -40,19 +40,19 @@ namespace photon { PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera) : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG(), - frc::AprilTagFieldLayout::LoadField( - frc::AprilTagField::kDefaultField)) {} + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::kDefaultField)) {} -PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera, - const SimCameraProperties& props, - const frc::AprilTagFieldLayout& tagLayout) +PhotonCameraSim::PhotonCameraSim( + PhotonCamera* camera, const SimCameraProperties& props, + const wpi::apriltag::AprilTagFieldLayout& tagLayout) : prop{props}, cam{camera}, tagLayout{tagLayout} { SetMinTargetAreaPixels(kDefaultMinAreaPx); videoSimRaw = - frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw", + wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), prop.GetResHeight()); - videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray); - videoSimProcessed = frc::CameraServer::PutVideo( + videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray); + videoSimProcessed = wpi::CameraServer::PutVideo( std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), prop.GetResHeight()); ts.subTable = cam->GetCameraTable(); @@ -62,21 +62,21 @@ PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera, PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, double minTargetAreaPercent, - units::meter_t maxSightRange) + wpi::units::meter_t maxSightRange) : PhotonCameraSim(camera, props) { this->minTargetAreaPercent = minTargetAreaPercent; this->maxSightRange = maxSightRange; } -bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose, +bool PhotonCameraSim::CanSeeTargetPose(const wpi::math::Pose3d& camPose, const VisionTargetSim& target) { CameraTargetRelation rel{camPose, target.GetPose()}; - return ((units::math::abs(rel.camToTargYaw.Degrees()) < + return ((wpi::units::math::abs(rel.camToTargYaw.Degrees()) < prop.GetHorizFOV().Degrees() / 2.0) && - (units::math::abs(rel.camToTargPitch.Degrees()) < + (wpi::units::math::abs(rel.camToTargPitch.Degrees()) < prop.GetVertFOV().Degrees() / 2.0) && (!target.GetModel().GetIsPlanar() || - units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && + wpi::units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && (rel.camToTarg.Translation().Norm() <= maxSightRange)); } bool PhotonCameraSim::CanSeeCorner(const std::vector& points) { @@ -89,13 +89,13 @@ bool PhotonCameraSim::CanSeeCorner(const std::vector& points) { return true; } std::optional PhotonCameraSim::ConsumeNextEntryTime() { - uint64_t now = wpi::Now(); + uint64_t now = wpi::util::Now(); uint64_t timestamp{}; int iter = 0; while (now >= nextNTEntryTime) { timestamp = nextNTEntryTime; uint64_t frameTime = prop.EstSecUntilNextFrame() - .convert() + .convert() .to(); nextNTEntryTime += frameTime; @@ -113,13 +113,13 @@ std::optional PhotonCameraSim::ConsumeNextEntryTime() { } } PhotonPipelineResult PhotonCameraSim::Process( - units::second_t latency, const frc::Pose3d& cameraPose, + wpi::units::second_t latency, const wpi::math::Pose3d& cameraPose, std::vector targets) { std::sort(targets.begin(), targets.end(), [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { - units::meter_t dist1 = + wpi::units::meter_t dist1 = t1.GetPose().Translation().Distance(cameraPose.Translation()); - units::meter_t dist2 = + wpi::units::meter_t dist2 = t2.GetPose().Translation().Distance(cameraPose.Translation()); return dist1 > dist2; }); @@ -144,7 +144,7 @@ PhotonPipelineResult PhotonCameraSim::Process( continue; } - std::vector fieldCorners = tgt.GetFieldVertices(); + std::vector fieldCorners = tgt.GetFieldVertices(); if (tgt.GetModel().GetIsSpherical()) { TargetModel model = tgt.GetModel(); fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose( @@ -186,11 +186,12 @@ PhotonPipelineResult PhotonCameraSim::Process( r = i; } } - cv::RotatedRect rect{ - cv::Point2d{center.x, center.y}, - cv::Size2d{imagePoints[r].x - lc.x, - imagePoints[b].y - imagePoints[t].y}, - units::radian_t{-angles[r]}.convert().to()}; + cv::RotatedRect rect{cv::Point2d{center.x, center.y}, + cv::Size2d{imagePoints[r].x - lc.x, + imagePoints[b].y - imagePoints[t].y}, + wpi::units::radian_t{-angles[r]} + .convert() + .to()}; std::vector points{}; rect.points(points); @@ -210,7 +211,7 @@ PhotonPipelineResult PhotonCameraSim::Process( minAreaRectPts.reserve(4); minAreaRect.points(minAreaRectPts); cv::Point2d centerPt = minAreaRect.center; - frc::Rotation3d centerRot = prop.GetPixelRot(centerPt); + wpi::math::Rotation3d centerRot = prop.GetPixelRot(centerPt); double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners); if (!(CanSeeCorner(noisyTargetCorners) && @@ -250,12 +251,12 @@ PhotonPipelineResult PhotonCameraSim::Process( std::vector cornersDouble{cornersFloat.begin(), cornersFloat.end()}; detectableTgts.emplace_back( - -centerRot.Z().convert().to(), - -centerRot.Y().convert().to(), areaPercent, - centerRot.X().convert().to(), + -centerRot.Z().convert().to(), + -centerRot.Y().convert().to(), areaPercent, + centerRot.X().convert().to(), tgt.GetFiducialId(), classId, conf, - pnpSim ? pnpSim->best : frc::Transform3d{}, - pnpSim ? pnpSim->alt : frc::Transform3d{}, + pnpSim ? pnpSim->best : wpi::math::Transform3d{}, + pnpSim ? pnpSim->alt : wpi::math::Transform3d{}, pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble); } @@ -288,7 +289,7 @@ PhotonPipelineResult PhotonCameraSim::Process( videoSimRaw.PutFrame(videoSimFrameRaw); } else { videoSimRaw.SetConnectionStrategy( - cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose); } if (videoSimProcEnabled) { @@ -333,19 +334,19 @@ PhotonPipelineResult PhotonCameraSim::Process( videoSimProcessed.PutFrame(videoSimFrameProcessed); } else { videoSimProcessed.SetConnectionStrategy( - cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose); } std::optional multiTagResults = std::nullopt; - std::vector visibleLayoutTags = + std::vector visibleLayoutTags = VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { std::vector usedIds{}; usedIds.resize(visibleLayoutTags.size()); std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), usedIds.begin(), - [](const frc::AprilTag& tag) { return tag.ID; }); + [](const wpi::apriltag::AprilTag& tag) { return tag.ID; }); std::sort(usedIds.begin(), usedIds.end()); auto pnpResult = VisionEstimation::EstimateCamPosePNP( prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, @@ -357,17 +358,17 @@ PhotonPipelineResult PhotonCameraSim::Process( return PhotonPipelineResult{ PhotonPipelineMetadata{heartbeatCounter, 0, - units::microsecond_t{latency}.to(), + wpi::units::microsecond_t{latency}.to(), 1000000}, detectableTgts, multiTagResults}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { - SubmitProcessedFrame(result, wpi::Now()); + SubmitProcessedFrame(result, wpi::util::Now()); } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, uint64_t ReceiveTimestamp) { ts.latencyMillisEntry.Set( - result.GetLatency().convert().to(), + result.GetLatency().convert().to(), ReceiveTimestamp); Packet newPacket{}; @@ -381,7 +382,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, ts.targetPitchEntry.Set(0.0, ReceiveTimestamp); ts.targetYawEntry.Set(0.0, ReceiveTimestamp); ts.targetAreaEntry.Set(0.0, ReceiveTimestamp); - ts.targetPoseEntry.Set(frc::Transform3d{}, ReceiveTimestamp); + ts.targetPoseEntry.Set(wpi::math::Transform3d{}, ReceiveTimestamp); ts.targetSkewEntry.Set(0.0, ReceiveTimestamp); } else { PhotonTrackedTarget bestTarget = result.GetBestTarget(); diff --git a/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp b/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp index 2099bdd815..e1555ce08a 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp @@ -33,20 +33,20 @@ using namespace photon; void SimCameraProperties::SetCalibration(int width, int height, - frc::Rotation2d fovDiag) { + wpi::math::Rotation2d fovDiag) { if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) { - fovDiag = frc::Rotation2d{ - std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; - FRC_ReportError( - frc::err::Error, + fovDiag = wpi::math::Rotation2d{ + std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; + WPILIB_ReportError( + wpi::err::Error, "Requested invalid FOV! Clamping between (1, 179) degrees..."); } double resDiag = std::sqrt(width * width + height * height); - double diagRatio = units::math::tan(fovDiag.Radians() / 2.0); - frc::Rotation2d fovWidth{ - units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; - frc::Rotation2d fovHeight{ - units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; + double diagRatio = wpi::units::math::tan(fovDiag.Radians() / 2.0); + wpi::math::Rotation2d fovWidth{ + wpi::units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; + wpi::math::Rotation2d fovHeight{ + wpi::units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; Eigen::Matrix newDistCoeffs = Eigen::Matrix::Zero(); @@ -54,8 +54,8 @@ void SimCameraProperties::SetCalibration(int width, int height, double cx = width / 2.0 - 0.5; double cy = height / 2.0 - 0.5; - double fx = cx / units::math::tan(fovWidth.Radians() / 2.0); - double fy = cy / units::math::tan(fovHeight.Radians() / 2.0); + double fx = cx / wpi::units::math::tan(fovWidth.Radians() / 2.0); + double fy = cy / wpi::units::math::tan(fovHeight.Radians() / 2.0); Eigen::Matrix newCamIntrinsics; newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; @@ -70,29 +70,37 @@ void SimCameraProperties::SetCalibration( camIntrinsics = newCamIntrinsics; distCoeffs = newDistCoeffs; - std::array p{ - frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad, - (GetPixelYaw(0) + - frc::Rotation2d{units::radian_t{ - -std::numbers::pi / 2.0}}) - .Radians()}}, - frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad, - (GetPixelYaw(width) + - frc::Rotation2d{units::radian_t{ - std::numbers::pi / 2.0}}) - .Radians()}}, - frc::Translation3d{1_m, frc::Rotation3d{0_rad, - (GetPixelPitch(0) + - frc::Rotation2d{units::radian_t{ - std::numbers::pi / 2.0}}) - .Radians(), - 0_rad}}, - frc::Translation3d{1_m, frc::Rotation3d{0_rad, - (GetPixelPitch(height) + - frc::Rotation2d{units::radian_t{ - -std::numbers::pi / 2.0}}) - .Radians(), - 0_rad}}, + std::array p{ + wpi::math::Translation3d{ + 1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, + (GetPixelYaw(0) + + wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}}) + .Radians()}}, + wpi::math::Translation3d{ + 1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, + (GetPixelYaw(width) + + wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}}) + .Radians()}}, + wpi::math::Translation3d{ + 1_m, + wpi::math::Rotation3d{ + 0_rad, + (GetPixelPitch(0) + + wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + wpi::math::Translation3d{ + 1_m, + wpi::math::Rotation3d{ + 0_rad, + (GetPixelPitch(height) + + wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, }; viewplanes.clear(); for (size_t i = 0; i < p.size(); i++) { @@ -103,10 +111,10 @@ void SimCameraProperties::SetCalibration( std::pair, std::optional> SimCameraProperties::GetVisibleLine(const RotTrlTransform3d& camRt, - const frc::Translation3d& a, - const frc::Translation3d& b) const { - frc::Translation3d relA = camRt.Apply(a); - frc::Translation3d relB = camRt.Apply(b); + const wpi::math::Translation3d& a, + const wpi::math::Translation3d& b) const { + wpi::math::Translation3d relA = camRt.Apply(a); + wpi::math::Translation3d relB = camRt.Apply(b); if (relA.X() <= 0_m && relB.X() <= 0_m) { return {std::nullopt, std::nullopt}; diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 6a0c2f8c4b..f5442cc81f 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,16 +28,17 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "photon/targeting/PhotonPipelineResult.h" @@ -63,7 +64,7 @@ class PhotonCamera { * @param cameraName The name of the camera, as seen in the UI. * over. */ - explicit PhotonCamera(nt::NetworkTableInstance instance, + explicit PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName); /** @@ -203,55 +204,55 @@ class PhotonCamera { */ static void SetVersionCheckEnabled(bool enabled); - std::shared_ptr GetCameraTable() const { return rootTable; } + std::shared_ptr GetCameraTable() const { return rootTable; } // For use in tests bool test = false; std::vector testResult; protected: - std::shared_ptr mainTable; - std::shared_ptr rootTable; - nt::RawSubscriber rawBytesEntry; - nt::IntegerPublisher inputSaveImgEntry; - nt::IntegerSubscriber inputSaveImgSubscriber; - nt::IntegerPublisher outputSaveImgEntry; - nt::IntegerSubscriber outputSaveImgSubscriber; - nt::IntegerPublisher pipelineIndexPub; - nt::IntegerSubscriber pipelineIndexSub; - nt::IntegerPublisher ledModePub; - nt::IntegerSubscriber ledModeSub; - nt::StringSubscriber versionEntry; + std::shared_ptr mainTable; + std::shared_ptr rootTable; + wpi::nt::RawSubscriber rawBytesEntry; + wpi::nt::IntegerPublisher inputSaveImgEntry; + wpi::nt::IntegerSubscriber inputSaveImgSubscriber; + wpi::nt::IntegerPublisher outputSaveImgEntry; + wpi::nt::IntegerSubscriber outputSaveImgSubscriber; + wpi::nt::IntegerPublisher pipelineIndexPub; + wpi::nt::IntegerSubscriber pipelineIndexSub; + wpi::nt::IntegerPublisher ledModePub; + wpi::nt::IntegerSubscriber ledModeSub; + wpi::nt::StringSubscriber versionEntry; - nt::DoubleArraySubscriber cameraIntrinsicsSubscriber; - nt::DoubleArraySubscriber cameraDistortionSubscriber; + wpi::nt::DoubleArraySubscriber cameraIntrinsicsSubscriber; + wpi::nt::DoubleArraySubscriber cameraDistortionSubscriber; - nt::BooleanSubscriber driverModeSubscriber; - nt::BooleanPublisher driverModePublisher; - nt::IntegerSubscriber fpsLimitSubscriber; - nt::IntegerPublisher fpsLimitPublisher; + wpi::nt::BooleanSubscriber driverModeSubscriber; + wpi::nt::BooleanPublisher driverModePublisher; + wpi::nt::IntegerSubscriber fpsLimitSubscriber; + wpi::nt::IntegerPublisher fpsLimitPublisher; - nt::IntegerSubscriber ledModeSubscriber; + wpi::nt::IntegerSubscriber ledModeSubscriber; - nt::IntegerSubscriber heartbeatSubscriber; + wpi::nt::IntegerSubscriber heartbeatSubscriber; - nt::MultiSubscriber topicNameSubscriber; + wpi::nt::MultiSubscriber topicNameSubscriber; std::string path; std::string cameraName; - frc::Alert disconnectAlert; - frc::Alert timesyncAlert; + wpi::Alert disconnectAlert; + wpi::Alert timesyncAlert; private: - units::second_t lastVersionCheckTime = 0_s; + wpi::units::second_t lastVersionCheckTime = 0_s; static bool VERSION_CHECK_ENABLED; inline static int InstanceCount = 1; - units::second_t prevTimeSyncWarnTime = 0_s; + wpi::units::second_t prevTimeSyncWarnTime = 0_s; int prevHeartbeatValue = -1; - units::second_t prevHeartbeatChangeTime = 0_s; + wpi::units::second_t prevHeartbeatChangeTime = 0_s; void VerifyVersion(); diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index d9d7bde518..14bf4eca17 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -24,12 +24,15 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include +#include + +#include +#include +#include +#include +#include +#include #include "photon/PhotonCamera.h" #include "photon/targeting/PhotonPipelineResult.h" @@ -55,18 +58,18 @@ struct ConstrainedSolvepnpParams { struct EstimatedRobotPose { /** The estimated pose */ - frc::Pose3d estimatedPose; + wpi::math::Pose3d estimatedPose; /** The estimated time the frame used to derive the robot pose was taken, in * the same timebase as the RoboRIO FPGA Timestamp */ - units::second_t timestamp; + wpi::units::second_t timestamp; /** A list of the targets used to compute this pose */ - wpi::SmallVector targetsUsed; + wpi::util::SmallVector targetsUsed; /** The strategy actually used to produce this pose */ PoseStrategy strategy; - EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_, + EstimatedRobotPose(wpi::math::Pose3d pose_, wpi::units::second_t time_, std::span targets, PoseStrategy strategy_) : estimatedPose(pose_), @@ -92,7 +95,7 @@ class PhotonPoseEstimator { * mount positions (ie, robot ➔ camera). */ explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, - frc::Transform3d robotToCamera); + wpi::math::Transform3d robotToCamera); /** * Create a new PhotonPoseEstimator. @@ -108,16 +111,18 @@ class PhotonPoseEstimator { [[deprecated( "Use individual estimation methods with the 2 argument constructor " "instead.")]] - explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, + explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags, PoseStrategy strategy, - frc::Transform3d robotToCamera); + wpi::math::Transform3d robotToCamera); /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * * @return the AprilTagFieldLayout */ - frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; } + wpi::apriltag::AprilTagFieldLayout GetFieldLayout() const { + return aprilTags; + } /** * Get the Position Estimation Strategy being used by the Position Estimator. @@ -161,7 +166,7 @@ class PhotonPoseEstimator { * @deprecated Use individual estimation methods instead. */ [[deprecated("Use individual estimation methods instead.")]] - frc::Pose3d GetReferencePose() const { + wpi::math::Pose3d GetReferencePose() const { return referencePose; } @@ -173,7 +178,7 @@ class PhotonPoseEstimator { * @deprecated Use individual estimation methods instead. */ [[deprecated("Use individual estimation methods instead.")]] - inline void SetReferencePose(frc::Pose3d referencePose) { + inline void SetReferencePose(wpi::math::Pose3d referencePose) { if (this->referencePose != referencePose) { InvalidatePoseCache(); } @@ -184,7 +189,7 @@ class PhotonPoseEstimator { * @return The current transform from the center of the robot to the camera * mount position. */ - inline frc::Transform3d GetRobotToCameraTransform() { + inline wpi::math::Transform3d GetRobotToCameraTransform() { return m_robotToCamera; } @@ -194,7 +199,7 @@ class PhotonPoseEstimator { * @param robotToCamera The current transform from the center of the robot to * the camera mount position. */ - inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) { + inline void SetRobotToCameraTransform(wpi::math::Transform3d robotToCamera) { m_robotToCamera = robotToCamera; } @@ -206,8 +211,10 @@ class PhotonPoseEstimator { * @deprecated Use individual estimation methods instead. */ [[deprecated("Use individual estimation methods instead.")]] - inline void SetLastPose(frc::Pose3d lastPose) { + inline void SetLastPose(wpi::math::Pose3d lastPose) { + this->lastPose = lastPose; + } /** @@ -218,8 +225,8 @@ class PhotonPoseEstimator { * @param heading Field-relative heading at the given timestamp. Standard * WPILIB field coordinates. */ - inline void AddHeadingData(units::second_t timestamp, - frc::Rotation2d heading) { + inline void AddHeadingData(wpi::units::second_t timestamp, + wpi::math::Rotation2d heading) { this->headingBuffer.AddSample(timestamp, heading); } @@ -231,8 +238,8 @@ class PhotonPoseEstimator { * @param heading Field-relative heading at the given timestamp. Standard * WPILIB coordinates. */ - inline void AddHeadingData(units::second_t timestamp, - frc::Rotation3d heading) { + inline void AddHeadingData(wpi::units::second_t timestamp, + wpi::math::Rotation3d heading) { AddHeadingData(timestamp, heading.ToRotation2d()); } @@ -245,8 +252,8 @@ class PhotonPoseEstimator { * @param heading Field-relative robot heading at given timestamp. Standard * WPILIB field coordinates. */ - inline void ResetHeadingData(units::second_t timestamp, - frc::Rotation2d heading) { + inline void ResetHeadingData(wpi::units::second_t timestamp, + wpi::math::Rotation2d heading) { headingBuffer.Clear(); AddHeadingData(timestamp, heading); } @@ -260,8 +267,8 @@ class PhotonPoseEstimator { * @param heading Field-relative robot heading at given timestamp. Standard * WPILIB field coordinates. */ - inline void ResetHeadingData(units::second_t timestamp, - frc::Rotation3d heading) { + inline void ResetHeadingData(wpi::units::second_t timestamp, + wpi::math::Rotation3d heading) { ResetHeadingData(timestamp, heading.ToRotation2d()); } @@ -322,7 +329,7 @@ class PhotonPoseEstimator { * targets used to create the estimate, or std::nullopt if there's no targets. */ std::optional EstimateClosestToReferencePose( - PhotonPipelineResult cameraResult, frc::Pose3d referencePose); + PhotonPipelineResult cameraResult, wpi::math::Pose3d referencePose); /** * Return the estimated position of the robot by using all visible tags to @@ -410,22 +417,22 @@ class PhotonPoseEstimator { std::optional EstimateConstrainedSolvepnpPose( photon::PhotonPipelineResult cameraResult, photon::PhotonCamera::CameraMatrix cameraMatrix, - photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose, + photon::PhotonCamera::DistortionMatrix distCoeffs, wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor); private: - frc::AprilTagFieldLayout aprilTags; + wpi::apriltag::AprilTagFieldLayout aprilTags; PoseStrategy strategy; PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY; - frc::Transform3d m_robotToCamera; + wpi::math::Transform3d m_robotToCamera; - frc::Pose3d lastPose; - frc::Pose3d referencePose; + wpi::math::Pose3d lastPose; + wpi::math::Pose3d referencePose; - units::second_t poseCacheTimestamp; + wpi::units::second_t poseCacheTimestamp; - frc::TimeInterpolatableBuffer headingBuffer; + wpi::math::TimeInterpolatableBuffer headingBuffer; inline static int InstanceCount = 1; diff --git a/photon-lib/src/main/native/include/photon/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h index 8561b3443d..ef87f01a59 100644 --- a/photon-lib/src/main/native/include/photon/PhotonUtils.h +++ b/photon-lib/src/main/native/include/photon/PhotonUtils.h @@ -24,13 +24,13 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include namespace photon { class PhotonUtils { @@ -50,12 +50,12 @@ class PhotonUtils { * values up. * @return The estimated distance to the target. */ - static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, - units::meter_t targetHeight, - units::radian_t cameraPitch, - units::radian_t targetPitch) { + static wpi::units::meter_t CalculateDistanceToTarget(wpi::units::meter_t cameraHeight, + wpi::units::meter_t targetHeight, + wpi::units::radian_t cameraPitch, + wpi::units::radian_t targetPitch) { return (targetHeight - cameraHeight) / - units::math::tan(cameraPitch + targetPitch); + wpi::units::math::tan(cameraPitch + targetPitch); } /** @@ -66,8 +66,8 @@ class PhotonUtils { * * @return The target's camera-relative translation. */ - static frc::Translation2d EstimateCameraToTargetTranslation( - units::meter_t targetDistance, const frc::Rotation2d& yaw) { + static wpi::math::Translation2d EstimateCameraToTargetTranslation( + wpi::units::meter_t targetDistance, const wpi::math::Rotation2d& yaw) { return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()}; } @@ -89,20 +89,22 @@ class PhotonUtils { * CW-positive. * @param gyroAngle The current robot gyro angle, likely from * odometry. - * @param fieldToTarget A frc::Pose2d representing the target position in - * the field coordinate system. + * @param fieldToTarget A wpi::math::Pose2d representing the target + * position in the field coordinate system. * @param cameraToRobot The position of the robot relative to the camera. * If the camera was mounted 3 inches behind the * "origin" (usually physical center) of the robot, - * this would be frc::Transform2d(3 inches, 0 + * this would be wpi::math::Transform2d(3 inches, 0 * inches, 0 degrees). * @return The position of the robot in the field. */ - static frc::Pose2d EstimateFieldToRobot( - units::meter_t cameraHeight, units::meter_t targetHeight, - units::radian_t cameraPitch, units::radian_t targetPitch, - const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, - const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) { + static wpi::math::Pose2d EstimateFieldToRobot( + wpi::units::meter_t cameraHeight, wpi::units::meter_t targetHeight, + wpi::units::radian_t cameraPitch, wpi::units::radian_t targetPitch, + const wpi::math::Rotation2d& targetYaw, + const wpi::math::Rotation2d& gyroAngle, + const wpi::math::Pose2d& fieldToTarget, + const wpi::math::Transform2d& cameraToRobot) { return EstimateFieldToRobot( EstimateCameraToTarget( EstimateCameraToTargetTranslation( @@ -114,30 +116,32 @@ class PhotonUtils { } /** - * Estimates a {@link frc::Transform2d} that maps the camera position to the - * target position, using the robot's gyro. Note that the gyro angle provided - * *must* line up with the field coordinate system -- that is, it should read - * zero degrees when pointed towards the opposing alliance station, and - * increase as the robot rotates CCW. + * Estimates a {@link wpi::math::Transform2d} that maps the camera position to + * the target position, using the robot's gyro. Note that the gyro angle + * provided *must* line up with the field coordinate system -- that is, it + * should read zero degrees when pointed towards the opposing alliance + * station, and increase as the robot rotates CCW. * * @param cameraToTargetTranslation A Translation2d that encodes the x/y * position of the target relative to the * camera. - * @param fieldToTarget A frc::Pose2d representing the target - * position in the field coordinate system. + * @param fieldToTarget A wpi::math::Pose2d representing the + * target position in the field coordinate system. * @param gyroAngle The current robot gyro angle, likely from * odometry. - * @return A frc::Transform2d that takes us from the camera to the target. + * @return A wpi::math::Transform2d that takes us from the camera to the + * target. */ - static frc::Transform2d EstimateCameraToTarget( - const frc::Translation2d& cameraToTargetTranslation, - const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) { + static wpi::math::Transform2d EstimateCameraToTarget( + const wpi::math::Translation2d& cameraToTargetTranslation, + const wpi::math::Pose2d& fieldToTarget, + const wpi::math::Rotation2d& gyroAngle) { // This pose maps our camera at the origin out to our target, in the robot // reference frame - // The translation part of this frc::Transform2d is from the above step, and - // the rotation uses our robot's gyro. - return frc::Transform2d(cameraToTargetTranslation, - -gyroAngle - fieldToTarget.Rotation()); + // The translation part of this wpi::math::Transform2d is from the above + // step, and the rotation uses our robot's gyro. + return wpi::math::Transform2d(cameraToTargetTranslation, + -gyroAngle - fieldToTarget.Rotation()); } /** @@ -150,12 +154,14 @@ class PhotonUtils { * @param cameraToRobot The position of the robot relative to the camera. If * the camera was mounted 3 inches behind the "origin" * (usually physical center) of the robot, this would be - * frc::Transform2d(3 inches, 0 inches, 0 degrees). + * wpi::math::Transform2d(3 inches, 0 inches, 0 + * degrees). * @return The position of the robot in the field. */ - static frc::Pose2d EstimateFieldToRobot( - const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget, - const frc::Transform2d& cameraToRobot) { + static wpi::math::Pose2d EstimateFieldToRobot( + const wpi::math::Transform2d& cameraToTarget, + const wpi::math::Pose2d& fieldToTarget, + const wpi::math::Transform2d& cameraToRobot) { return EstimateFieldToCamera(cameraToTarget, fieldToTarget) .TransformBy(cameraToRobot); } @@ -170,9 +176,9 @@ class PhotonUtils { * @param fieldToTarget The position of the target in the field. * @return The position of the camera in the field. */ - static frc::Pose2d EstimateFieldToCamera( - const frc::Transform2d& cameraToTarget, - const frc::Pose2d& fieldToTarget) { + static wpi::math::Pose2d EstimateFieldToCamera( + const wpi::math::Transform2d& cameraToTarget, + const wpi::math::Pose2d& fieldToTarget) { auto targetToCamera = cameraToTarget.Inverse(); return fieldToTarget.TransformBy(targetToCamera); } diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index d70d9f8994..d19fd4751b 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -27,15 +27,16 @@ #include #include -#include -#include -#include #include #include #include #include -#include -#include +#include +#include +#include +#include +#include +#include namespace photon { class PhotonCameraSim { @@ -68,9 +69,9 @@ class PhotonCameraSim { * positions. */ PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, - const frc::AprilTagFieldLayout& tagLayout = - frc::AprilTagFieldLayout::LoadField( - frc::AprilTagField::kDefaultField)); + const wpi::apriltag::AprilTagFieldLayout& tagLayout = + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::kDefaultField)); /** * Constructs a handle for simulating PhotonCamera values. Processing @@ -87,7 +88,7 @@ class PhotonCameraSim { * separate from this. */ PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, - double minTargetAreaPercent, units::meter_t maxSightRange); + double minTargetAreaPercent, wpi::units::meter_t maxSightRange); /** * Returns the camera being simulated. @@ -120,8 +121,8 @@ class PhotonCameraSim { * * @return The distance */ - inline units::meter_t GetMaxSightRange() { return maxSightRange; } - inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } + inline wpi::units::meter_t GetMaxSightRange() { return maxSightRange; } + inline const wpi::cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } inline const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; } /** @@ -132,7 +133,7 @@ class PhotonCameraSim { * @param target Vision target containing pose and shape * @return If this vision target can be seen before image projection. */ - bool CanSeeTargetPose(const frc::Pose3d& camPose, + bool CanSeeTargetPose(const wpi::math::Pose3d& camPose, const VisionTargetSim& target); /** @@ -182,7 +183,7 @@ class PhotonCameraSim { * * @param rangeMeters The distance */ - inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + inline void SetMaxSightRange(wpi::units::meter_t range) { maxSightRange = range; } /** * Sets whether the raw video stream simulation is enabled. @@ -225,8 +226,8 @@ class PhotonCameraSim { inline void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } - PhotonPipelineResult Process(units::second_t latency, - const frc::Pose3d& cameraPose, + PhotonPipelineResult Process(wpi::units::second_t latency, + const wpi::math::Pose3d& cameraPose, std::vector targets); void SubmitProcessedFrame(const PhotonPipelineResult& result); @@ -241,20 +242,20 @@ class PhotonCameraSim { NTTopicSet ts{}; int64_t heartbeatCounter{0}; - uint64_t nextNTEntryTime{wpi::Now()}; + uint64_t nextNTEntryTime{wpi::util::Now()}; - units::meter_t maxSightRange{std::numeric_limits::max()}; + wpi::units::meter_t maxSightRange{std::numeric_limits::max()}; static constexpr double kDefaultMinAreaPx{100}; double minTargetAreaPercent; - frc::AprilTagFieldLayout tagLayout; + wpi::apriltag::AprilTagFieldLayout tagLayout; - cs::CvSource videoSimRaw; + wpi::cs::CvSource videoSimRaw; cv::Mat videoSimFrameRaw{}; bool videoSimRawEnabled{true}; bool videoSimWireframeEnabled{false}; double videoSimWireframeResolution{0.1}; - cs::CvSource videoSimProcessed; + wpi::cs::CvSource videoSimProcessed; cv::Mat videoSimFrameProcessed{}; bool videoSimProcEnabled{true}; }; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index 4ca5d8d6a1..daa0df98b7 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -30,11 +30,11 @@ #include #include -#include -#include #include -#include -#include +#include +#include +#include +#include namespace photon { @@ -56,7 +56,9 @@ namespace photon { class SimCameraProperties { public: /** Default constructor which is the same as PERFECT_90DEG */ - SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + SimCameraProperties() { + SetCalibration(960, 720, wpi::math::Rotation2d{90_deg}); + } /** * Reads camera properties from a PhotonVision config.json file. @@ -69,7 +71,7 @@ class SimCameraProperties { */ SimCameraProperties(std::string path, int width, int height) {} - void SetCalibration(int width, int height, frc::Rotation2d fovDiag); + void SetCalibration(int width, int height, wpi::math::Rotation2d fovDiag); void SetCalibration(int width, int height, const Eigen::Matrix& newCamIntrinsics, const Eigen::Matrix& newDistCoeffs); @@ -85,8 +87,8 @@ class SimCameraProperties { * @param fps The average frames per second the camera should process at. * **Exposure time limits FPS if set!** */ - void SetFPS(units::hertz_t fps) { - frameSpeed = units::math::max(1 / fps, exposureTime); + void SetFPS(wpi::units::hertz_t fps) { + frameSpeed = wpi::units::math::max(1 / fps, exposureTime); } /** @@ -95,9 +97,9 @@ class SimCameraProperties { * @param exposureTime The amount of time the "shutter" is open for one frame. * Affects motion blur. **Frame speed(from FPS) is limited to this!** */ - void SetExposureTime(units::second_t exposureTime) { + void SetExposureTime(wpi::units::second_t exposureTime) { this->exposureTime = exposureTime; - frameSpeed = units::math::max(frameSpeed, this->exposureTime); + frameSpeed = wpi::units::math::max(frameSpeed, this->exposureTime); } /** @@ -106,7 +108,7 @@ class SimCameraProperties { * @param avgLatency The average latency (from image capture to data * published) a frame should have */ - void SetAvgLatency(units::second_t avgLatency) { + void SetAvgLatency(wpi::units::second_t avgLatency) { this->avgLatency = avgLatency; } @@ -115,7 +117,7 @@ class SimCameraProperties { * * @param latencyStdDev The standard deviation of the latency */ - void SetLatencyStdDev(units::second_t latencyStdDev) { + void SetLatencyStdDev(wpi::units::second_t latencyStdDev) { this->latencyStdDev = latencyStdDev; } @@ -159,35 +161,35 @@ class SimCameraProperties { * * @return The FPS */ - units::hertz_t GetFPS() const { return 1 / frameSpeed; } + wpi::units::hertz_t GetFPS() const { return 1 / frameSpeed; } /** * Gets the time per frame of the simulated camera. * * @return The time per frame */ - units::second_t GetFrameSpeed() const { return frameSpeed; } + wpi::units::second_t GetFrameSpeed() const { return frameSpeed; } /** * Gets the exposure time of the simulated camera. * * @return The exposure time */ - units::second_t GetExposureTime() const { return exposureTime; } + wpi::units::second_t GetExposureTime() const { return exposureTime; } /** * Gets the average latency of the simulated camera. * * @return The average latency */ - units::second_t GetAverageLatency() const { return avgLatency; } + wpi::units::second_t GetAverageLatency() const { return avgLatency; } /** * Gets the time per frame of the simulated camera. * * @return The time per frame */ - units::second_t GetLatencyStdDev() const { return latencyStdDev; } + wpi::units::second_t GetLatencyStdDev() const { return latencyStdDev; } /** * The percentage (0 - 100) of this camera's resolution the contour takes up @@ -203,11 +205,11 @@ class SimCameraProperties { /** The yaw from the principal point of this camera to the pixel x value. * Positive values left. */ - frc::Rotation2d GetPixelYaw(double pixelX) const { + wpi::math::Rotation2d GetPixelYaw(double pixelX) const { double fx = camIntrinsics(0, 0); double cx = camIntrinsics(0, 2); double xOffset = cx - pixelX; - return frc::Rotation2d{fx, xOffset}; + return wpi::math::Rotation2d{fx, xOffset}; } /** @@ -217,11 +219,11 @@ class SimCameraProperties { * Note that this angle is naively computed and may be incorrect. See * #getCorrectedPixelRot(const cv::Point2d). */ - frc::Rotation2d GetPixelPitch(double pixelY) const { + wpi::math::Rotation2d GetPixelPitch(double pixelY) const { double fy = camIntrinsics(1, 1); double cy = camIntrinsics(1, 2); double yOffset = cy - pixelY; - return frc::Rotation2d{fy, -yOffset}; + return wpi::math::Rotation2d{fy, -yOffset}; } /** * Finds the yaw and pitch to the given image point. Yaw is positive left, and @@ -230,9 +232,9 @@ class SimCameraProperties { * Note that pitch is naively computed and may be incorrect. See * #getCorrectedPixelRot(const cv::Point2d). */ - frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { - return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), - GetPixelYaw(point.x).Radians()}; + wpi::math::Rotation3d GetPixelRot(const cv::Point2d& point) const { + return wpi::math::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), + GetPixelYaw(point.x).Radians()}; } /** @@ -260,7 +262,7 @@ class SimCameraProperties { * @return Rotation3d with yaw and pitch of the line projected out of the * camera from the given pixel (roll is zero). */ - frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { + wpi::math::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { double fx = camIntrinsics(0, 0); double cx = camIntrinsics(0, 2); double xOffset = cx - point.x; @@ -269,26 +271,27 @@ class SimCameraProperties { double cy = camIntrinsics(1, 2); double yOffset = cy - point.y; - frc::Rotation2d yaw{fx, xOffset}; - frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset}; - return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; + wpi::math::Rotation2d yaw{fx, xOffset}; + wpi::math::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), + -yOffset}; + return wpi::math::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; } - frc::Rotation2d GetHorizFOV() const { - frc::Rotation2d left = GetPixelYaw(0); - frc::Rotation2d right = GetPixelYaw(resWidth); + wpi::math::Rotation2d GetHorizFOV() const { + wpi::math::Rotation2d left = GetPixelYaw(0); + wpi::math::Rotation2d right = GetPixelYaw(resWidth); return left - right; } - frc::Rotation2d GetVertFOV() const { - frc::Rotation2d above = GetPixelPitch(0); - frc::Rotation2d below = GetPixelPitch(resHeight); + wpi::math::Rotation2d GetVertFOV() const { + wpi::math::Rotation2d above = GetPixelPitch(0); + wpi::math::Rotation2d below = GetPixelPitch(resHeight); return below - above; } - frc::Rotation2d GetDiagFOV() const { - return frc::Rotation2d{ - units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + wpi::math::Rotation2d GetDiagFOV() const { + return wpi::math::Rotation2d{ + wpi::units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; } /** @@ -303,7 +306,7 @@ class SimCameraProperties { * inside the camera frustum, {0.5, 1} would be returned. * * @param camRt The change in basis from world coordinates to camera - * coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d). + * coordinates. See RotTrlTransform3d#makeRelativeTo(wpi::math::Pose3d). * @param a The initial translation of the line * @param b The final translation of the line * @return A Pair of Doubles. The values may be empty: @@ -316,8 +319,8 @@ class SimCameraProperties { * visible in the camera frustum. */ std::pair, std::optional> GetVisibleLine( - const RotTrlTransform3d& camRt, const frc::Translation3d& a, - const frc::Translation3d& b) const; + const RotTrlTransform3d& camRt, const wpi::math::Translation3d& a, + const wpi::math::Translation3d& b) const; /** * Returns these points after applying this camera's estimated noise. @@ -349,8 +352,8 @@ class SimCameraProperties { * * @return The latency estimate */ - units::second_t EstLatency() { - return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, + wpi::units::second_t EstLatency() { + return wpi::units::math::max(avgLatency + gaussian(generator) * latencyStdDev, 0_s); } @@ -359,8 +362,8 @@ class SimCameraProperties { * * @return The estimated time until the next frame */ - units::second_t EstSecUntilNextFrame() { - return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); + wpi::units::second_t EstSecUntilNextFrame() { + return frameSpeed + wpi::units::math::max(0_s, EstLatency() - frameSpeed); } /** @@ -532,10 +535,10 @@ class SimCameraProperties { Eigen::Matrix distCoeffs; double avgErrorPx{0}; double errorStdDevPx{0}; - units::second_t frameSpeed{0}; - units::second_t exposureTime{0}; - units::second_t avgLatency{0}; - units::second_t latencyStdDev{0}; + wpi::units::second_t frameSpeed{0}; + wpi::units::second_t exposureTime{0}; + wpi::units::second_t avgLatency{0}; + wpi::units::second_t latencyStdDev{0}; std::vector> viewplanes{}; }; } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h index b4c5c1a102..fcdf4a40a5 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -31,12 +31,12 @@ #include #include -#include -#include #include #include #include -#include +#include +#include +#include #include "photon/simulation/SimCameraProperties.h" @@ -53,12 +53,12 @@ namespace VideoSimUtil { // required static constexpr int kNumTags36h11 = 40; -static constexpr units::meter_t fieldLength{16.54175_m}; -static constexpr units::meter_t fieldWidth{8.0137_m}; +static constexpr wpi::units::meter_t fieldLength{16.54175_m}; +static constexpr wpi::units::meter_t fieldWidth{8.0137_m}; static cv::Mat Get36h11TagImage(int id) { - wpi::RawFrame frame; - frc::AprilTag::Generate36h11AprilTagImage(&frame, id); + wpi::util::RawFrame frame; + wpi::apriltag::AprilTag::Generate36h11AprilTagImage(&frame, id); cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data, static_cast(frame.stride)}; cv::Mat markerClone = markerImage.clone(); @@ -128,7 +128,7 @@ static const std::vector kTag36h11MarkPts = Get36h11MarkerPts(); /** Updates the properties of this cs::CvSource video stream with the given * camera properties. */ -[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, +[[maybe_unused]] static void UpdateVideoProp(wpi::cs::CvSource& video, const SimCameraProperties& prop) { video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); video.SetFPS(prop.GetFPS().to()); @@ -343,55 +343,56 @@ static void DrawPoly(const std::vector& dstPoints, int thickness, * The translations used to draw the field side walls and driver station walls. * It is a vector of vectors because the translations are not all connected. */ -static std::vector> GetFieldWallLines() { - std::vector> list; +static std::vector> GetFieldWallLines() { + std::vector> list; - const units::meter_t sideHt = 19.5_in; - const units::meter_t driveHt = 35_in; - const units::meter_t topHt = 78_in; + const wpi::units::meter_t sideHt = 19.5_in; + const wpi::units::meter_t driveHt = 35_in; + const wpi::units::meter_t topHt = 78_in; // field floor - list.emplace_back(std::vector{ - frc::Translation3d{0_m, 0_m, 0_m}, - frc::Translation3d{fieldLength, 0_m, 0_m}, - frc::Translation3d{fieldLength, fieldWidth, 0_m}, - frc::Translation3d{0_m, fieldWidth, 0_m}, - frc::Translation3d{0_m, 0_m, 0_m}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, 0_m, 0_m}, + wpi::math::Translation3d{fieldLength, 0_m, 0_m}, + wpi::math::Translation3d{fieldLength, fieldWidth, 0_m}, + wpi::math::Translation3d{0_m, fieldWidth, 0_m}, + wpi::math::Translation3d{0_m, 0_m, 0_m}}); // right side wall - list.emplace_back(std::vector{ - frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt}, - frc::Translation3d{fieldLength, 0_m, sideHt}, - frc::Translation3d{fieldLength, 0_m, 0_m}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, 0_m, 0_m}, + wpi::math::Translation3d{0_m, 0_m, sideHt}, + wpi::math::Translation3d{fieldLength, 0_m, sideHt}, + wpi::math::Translation3d{fieldLength, 0_m, 0_m}}); // red driverstation - list.emplace_back(std::vector{ - frc::Translation3d{fieldLength, 0_m, sideHt}, - frc::Translation3d{fieldLength, 0_m, topHt}, - frc::Translation3d{fieldLength, fieldWidth, topHt}, - frc::Translation3d{fieldLength, fieldWidth, sideHt}, + list.emplace_back(std::vector{ + wpi::math::Translation3d{fieldLength, 0_m, sideHt}, + wpi::math::Translation3d{fieldLength, 0_m, topHt}, + wpi::math::Translation3d{fieldLength, fieldWidth, topHt}, + wpi::math::Translation3d{fieldLength, fieldWidth, sideHt}, }); - list.emplace_back(std::vector{ - frc::Translation3d{fieldLength, 0_m, driveHt}, - frc::Translation3d{fieldLength, fieldWidth, driveHt}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{fieldLength, 0_m, driveHt}, + wpi::math::Translation3d{fieldLength, fieldWidth, driveHt}}); // left side wall - list.emplace_back(std::vector{ - frc::Translation3d{0_m, fieldWidth, 0_m}, - frc::Translation3d{0_m, fieldWidth, sideHt}, - frc::Translation3d{fieldLength, fieldWidth, sideHt}, - frc::Translation3d{fieldLength, fieldWidth, 0_m}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, fieldWidth, 0_m}, + wpi::math::Translation3d{0_m, fieldWidth, sideHt}, + wpi::math::Translation3d{fieldLength, fieldWidth, sideHt}, + wpi::math::Translation3d{fieldLength, fieldWidth, 0_m}}); // blue driverstation - list.emplace_back(std::vector{ - frc::Translation3d{0_m, 0_m, sideHt}, - frc::Translation3d{0_m, 0_m, topHt}, - frc::Translation3d{0_m, fieldWidth, topHt}, - frc::Translation3d{0_m, fieldWidth, sideHt}, + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, 0_m, sideHt}, + wpi::math::Translation3d{0_m, 0_m, topHt}, + wpi::math::Translation3d{0_m, fieldWidth, topHt}, + wpi::math::Translation3d{0_m, fieldWidth, sideHt}, }); - list.emplace_back(std::vector{ - frc::Translation3d{0_m, 0_m, driveHt}, - frc::Translation3d{0_m, fieldWidth, driveHt}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, 0_m, driveHt}, + wpi::math::Translation3d{0_m, fieldWidth, driveHt}}); return list; } @@ -405,19 +406,19 @@ static std::vector> GetFieldWallLines() { * floor. E.g. 3 subdivisions would mean 2 lines along the length and 2 lines * along the width creating a 3x3 "grid". */ -static std::vector> GetFieldFloorLines( +static std::vector> GetFieldFloorLines( int subdivisions) { - std::vector> list; - const units::meter_t subLength = fieldLength / subdivisions; - const units::meter_t subWidth = fieldWidth / subdivisions; + std::vector> list; + const wpi::units::meter_t subLength = fieldLength / subdivisions; + const wpi::units::meter_t subWidth = fieldWidth / subdivisions; for (int i = 0; i < subdivisions; i++) { - list.emplace_back(std::vector{ - frc::Translation3d{0_m, subWidth * (i + 1), 0_m}, - frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); - list.emplace_back(std::vector{ - frc::Translation3d{subLength * (i + 1), 0_m, 0_m}, - frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{0_m, subWidth * (i + 1), 0_m}, + wpi::math::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); + list.emplace_back(std::vector{ + wpi::math::Translation3d{subLength * (i + 1), 0_m, 0_m}, + wpi::math::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); } return list; } @@ -441,19 +442,19 @@ static std::vector> GetFieldFloorLines( */ static std::vector> PolyFrom3dLines( const RotTrlTransform3d& camRt, const SimCameraProperties& prop, - const std::vector& trls, double resolution, + const std::vector& trls, double resolution, bool isClosed, cv::Mat& destination) { resolution = std::hypot(destination.size().height, destination.size().width) * resolution; - std::vector pts{trls}; + std::vector pts{trls}; if (isClosed) { pts.emplace_back(pts[0]); } std::vector> polyPointList{}; for (size_t i = 0; i < pts.size() - 1; i++) { - frc::Translation3d pta = pts[i]; - frc::Translation3d ptb = pts[i + 1]; + wpi::math::Translation3d pta = pts[i]; + wpi::math::Translation3d ptb = pts[i + 1]; std::pair, std::optional> inter = prop.GetVisibleLine(camRt, pta, ptb); @@ -463,8 +464,8 @@ static std::vector> PolyFrom3dLines( double inter1 = inter.first.value(); double inter2 = inter.second.value(); - frc::Translation3d baseDelta = ptb - pta; - frc::Translation3d old_pta = pta; + wpi::math::Translation3d baseDelta = ptb - pta; + wpi::math::Translation3d old_pta = pta; if (inter1 > 0) { pta = old_pta + baseDelta * inter1; } @@ -480,8 +481,8 @@ static std::vector> PolyFrom3dLines( double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y); int subdivisions = static_cast(pxDist / resolution); - frc::Translation3d subDelta = baseDelta / (subdivisions + 1); - std::vector subPts{}; + wpi::math::Translation3d subDelta = baseDelta / (subdivisions + 1); + std::vector subPts{}; for (int j = 0; j < subdivisions; j++) { subPts.emplace_back(pta + (subDelta * (j + 1))); } @@ -501,7 +502,7 @@ static std::vector> PolyFrom3dLines( * Draw a wireframe of the field to the given image. * * @param camRt The change in basis from world coordinates to camera - * coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d). + * coordinates. See RotTrlTransform3d#makeRelativeTo(wpi::math::Pose3d). * @param prop The simulated camera's properties. * @param resolution Resolution as a fraction(0 - 1) of the video frame's * diagonal length in pixels. Line segments will be subdivided if they exceed diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h index 6e9da0d107..95a76326f5 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -29,11 +29,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include "photon/simulation/PhotonCameraSim.h" @@ -60,7 +60,7 @@ class VisionSystemSim { */ explicit VisionSystemSim(std::string visionSystemName) { std::string tableName = "VisionSystemSim-" + visionSystemName; - frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + wpi::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); } /** Get one of the simulated cameras. */ @@ -91,17 +91,18 @@ class VisionSystemSim { * @param robotToCamera The transform from the robot pose to the camera pose */ void AddCamera(PhotonCameraSim* cameraSim, - const frc::Transform3d& robotToCamera) { + const wpi::math::Transform3d& robotToCamera) { auto found = camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()}); if (found == camSimMap.end()) { camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] = cameraSim; - camTrfMap.insert(std::make_pair( - std::move(cameraSim), - frc::TimeInterpolatableBuffer{bufferLength})); - camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), - frc::Pose3d{} + robotToCamera); + camTrfMap.insert( + std::make_pair(std::move(cameraSim), + wpi::math::TimeInterpolatableBuffer{ + bufferLength})); + camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(), + wpi::math::Pose3d{} + robotToCamera); } } @@ -135,8 +136,9 @@ class VisionSystemSim { * of * @return The transform of this camera, or an empty optional if it is invalid */ - std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { - return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp()); + std::optional GetRobotToCamera( + PhotonCameraSim* cameraSim) { + return GetRobotToCamera(cameraSim, wpi::Timer::GetFPGATimestamp()); } /** @@ -148,17 +150,17 @@ class VisionSystemSim { * @param time Timestamp of when the transform should be observed * @return The transform of this camera, or an empty optional if it is invalid */ - std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, - units::second_t time) { + std::optional GetRobotToCamera( + PhotonCameraSim* cameraSim, wpi::units::second_t time) { if (camTrfMap.find(cameraSim) != camTrfMap.end()) { - frc::TimeInterpolatableBuffer trfBuffer = + wpi::math::TimeInterpolatableBuffer trfBuffer = camTrfMap.at(cameraSim); - std::optional sample = trfBuffer.Sample(time); + std::optional sample = trfBuffer.Sample(time); if (!sample) { return std::nullopt; } else { - return std::make_optional( - frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})}); + return std::make_optional(wpi::math::Transform3d{ + wpi::math::Pose3d{}, sample.value_or(wpi::math::Pose3d{})}); } } else { return std::nullopt; @@ -172,8 +174,8 @@ class VisionSystemSim { * @param cameraSim The specific camera to get the field pose of * @return The pose of this camera, or an empty optional if it is invalid */ - std::optional GetCameraPose(PhotonCameraSim* cameraSim) { - return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); + std::optional GetCameraPose(PhotonCameraSim* cameraSim) { + return GetCameraPose(cameraSim, wpi::Timer::GetFPGATimestamp()); } /** @@ -184,8 +186,8 @@ class VisionSystemSim { * @param time Timestamp of when the pose should be observed * @return The pose of this camera, or an empty optional if it is invalid */ - std::optional GetCameraPose(PhotonCameraSim* cameraSim, - units::second_t time) { + std::optional GetCameraPose(PhotonCameraSim* cameraSim, + wpi::units::second_t time) { auto robotToCamera = GetRobotToCamera(cameraSim, time); if (!robotToCamera) { return std::nullopt; @@ -203,10 +205,10 @@ class VisionSystemSim { * @return If the cameraSim was valid and transform was adjusted */ bool AdjustCamera(PhotonCameraSim* cameraSim, - const frc::Transform3d& robotToCamera) { + const wpi::math::Transform3d& robotToCamera) { if (camTrfMap.find(cameraSim) != camTrfMap.end()) { - camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), - frc::Pose3d{} + robotToCamera); + camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(), + wpi::math::Pose3d{} + robotToCamera); return true; } else { return false; @@ -228,11 +230,12 @@ class VisionSystemSim { * @return If the cameraSim was valid and transforms were reset */ bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { - units::second_t now = frc::Timer::GetFPGATimestamp(); + wpi::units::second_t now = wpi::Timer::GetFPGATimestamp(); if (camTrfMap.find(cameraSim) != camTrfMap.end()) { auto trfBuffer = camTrfMap.at(cameraSim); - frc::Transform3d lastTrf{frc::Pose3d{}, - trfBuffer.Sample(now).value_or(frc::Pose3d{})}; + wpi::math::Transform3d lastTrf{ + wpi::math::Pose3d{}, + trfBuffer.Sample(now).value_or(wpi::math::Pose3d{})}; trfBuffer.Clear(); AdjustCamera(cameraSim, lastTrf); return true; @@ -312,9 +315,9 @@ class VisionSystemSim { * * @param layout The field tag layout to get Apriltag poses and IDs from */ - void AddAprilTags(const frc::AprilTagFieldLayout& layout) { + void AddAprilTags(const wpi::apriltag::AprilTagFieldLayout& layout) { std::vector targets; - for (const frc::AprilTag& tag : layout.GetTags()) { + for (const wpi::apriltag::AprilTag& tag : layout.GetTags()) { targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), photon::kAprilTag36h11, tag.ID}); } @@ -365,8 +368,8 @@ class VisionSystemSim { * * @return The latest robot pose */ - frc::Pose3d GetRobotPose() { - return GetRobotPose(frc::Timer::GetFPGATimestamp()); + wpi::math::Pose3d GetRobotPose() { + return GetRobotPose(wpi::Timer::GetFPGATimestamp()); } /** @@ -375,8 +378,8 @@ class VisionSystemSim { * @param timestamp Timestamp of the desired robot pose * @return The robot pose */ - frc::Pose3d GetRobotPose(units::second_t timestamp) { - return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); + wpi::math::Pose3d GetRobotPose(wpi::units::second_t timestamp) { + return robotPoseBuffer.Sample(timestamp).value_or(wpi::math::Pose3d{}); } /** @@ -384,8 +387,8 @@ class VisionSystemSim { * * @param robotPose The robot pose */ - void ResetRobotPose(const frc::Pose2d& robotPose) { - ResetRobotPose(frc::Pose3d{robotPose}); + void ResetRobotPose(const wpi::math::Pose2d& robotPose) { + ResetRobotPose(wpi::math::Pose3d{robotPose}); } /** @@ -393,11 +396,11 @@ class VisionSystemSim { * * @param robotPose The robot pose */ - void ResetRobotPose(const frc::Pose3d& robotPose) { + void ResetRobotPose(const wpi::math::Pose3d& robotPose) { robotPoseBuffer.Clear(); - robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); + robotPoseBuffer.AddSample(wpi::Timer::GetFPGATimestamp(), robotPose); } - frc::Field2d& GetDebugField() { return dbgField; } + wpi::Field2d& GetDebugField() { return dbgField; } /** * Periodic update. Ensure this is called repeatedly-- camera performance is @@ -405,7 +408,9 @@ class VisionSystemSim { * * @param robotPoseMeters The simulated robot pose in meters */ - void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + void Update(const wpi::math::Pose2d& robotPose) { + Update(wpi::math::Pose3d{robotPose}); + } /** * Periodic update. Ensure this is called repeatedly-- camera performance is @@ -413,16 +418,16 @@ class VisionSystemSim { * * @param robotPoseMeters The simulated robot pose in meters */ - void Update(const frc::Pose3d& robotPose) { + void Update(const wpi::math::Pose3d& robotPose) { for (auto& set : targetSets) { - std::vector posesToAdd{}; + std::vector posesToAdd{}; for (auto& target : set.second) { posesToAdd.emplace_back(target.GetPose().ToPose2d()); } dbgField.GetObject(set.first)->SetPoses(posesToAdd); } - units::second_t now = frc::Timer::GetFPGATimestamp(); + wpi::units::second_t now = wpi::Timer::GetFPGATimestamp(); robotPoseBuffer.AddSample(now, robotPose); dbgField.SetRobotPose(robotPose.ToPose2d()); @@ -433,8 +438,8 @@ class VisionSystemSim { } } - std::vector visTgtPoses2d{}; - std::vector cameraPoses2d{}; + std::vector visTgtPoses2d{}; + std::vector cameraPoses2d{}; bool processed{false}; for (const auto& entry : camSimMap) { auto camSim = entry.second; @@ -445,12 +450,12 @@ class VisionSystemSim { processed = true; } uint64_t timestampNt = optTimestamp.value(); - units::second_t latency = camSim->prop.EstLatency(); - units::second_t timestampCapture = - units::microsecond_t{static_cast(timestampNt)} - latency; + wpi::units::second_t latency = camSim->prop.EstLatency(); + wpi::units::second_t timestampCapture = + wpi::units::microsecond_t{static_cast(timestampNt)} - latency; - frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture); - frc::Pose3d lateCameraPose = + wpi::math::Pose3d lateRobotPose = GetRobotPose(timestampCapture); + wpi::math::Pose3d lateCameraPose = lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); cameraPoses2d.push_back(lateCameraPose.ToPose2d()); @@ -474,13 +479,14 @@ class VisionSystemSim { private: std::unordered_map camSimMap{}; - static constexpr units::second_t bufferLength{1.5_s}; + static constexpr wpi::units::second_t bufferLength{1.5_s}; std::unordered_map> + wpi::math::TimeInterpolatableBuffer> camTrfMap; - frc::TimeInterpolatableBuffer robotPoseBuffer{bufferLength}; + wpi::math::TimeInterpolatableBuffer robotPoseBuffer{ + bufferLength}; std::unordered_map> targetSets{}; - frc::Field2d dbgField{}; - const frc::Transform3d kEmptyTrf{}; + wpi::Field2d dbgField{}; + const wpi::math::Transform3d kEmptyTrf{}; }; } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h index 5095c5e985..7f91b4eb0f 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h @@ -26,7 +26,7 @@ #include -#include +#include #include "photon/estimation/TargetModel.h" @@ -42,7 +42,7 @@ class VisionTargetSim { * @param pose Pose3d of the tag in field-relative coordinates * @param model TargetModel which describes the geometry of the target */ - VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model) + VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model) : fiducialId(-1), objDetClassId(-1), objDetConf(-1), @@ -57,7 +57,8 @@ class VisionTargetSim { * @param model TargetModel which describes the geometry of the target(tag) * @param id The ID of this fiducial tag */ - VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id) + VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model, + int id) : fiducialId(id), objDetClassId(-1), objDetConf(-1), @@ -79,7 +80,7 @@ class VisionTargetSim { * simulation will compute a confidence based on the area of the target in the * camera's field of view */ - VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, + VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model, int objDetClassId, float objDetConf) : fiducialId(-1), objDetClassId(objDetClassId), @@ -92,7 +93,7 @@ class VisionTargetSim { * * @param newPose The pose in field-relative coordinates */ - void SetPose(const frc::Pose3d& newPose) { pose = newPose; } + void SetPose(const wpi::math::Pose3d& newPose) { pose = newPose; } /** * Sets the model describing this target's geometry. @@ -106,7 +107,7 @@ class VisionTargetSim { * * @return The pose in field-relative coordinates */ - frc::Pose3d GetPose() const { return pose; } + wpi::math::Pose3d GetPose() const { return pose; } /** * Returns the model describing this target's geometry. @@ -142,7 +143,7 @@ class VisionTargetSim { * This target's vertices offset from its field pose. * @return A vector of Translation3d representing the vertices of the target */ - std::vector GetFieldVertices() const { + std::vector GetFieldVertices() const { return model.GetFieldVertices(pose); } @@ -151,18 +152,18 @@ class VisionTargetSim { } bool operator==(const VisionTargetSim& other) const { - return units::math::abs(pose.Translation().X() - - other.GetPose().Translation().X()) < 1_in && - units::math::abs(pose.Translation().Y() - - other.GetPose().Translation().Y()) < 1_in && - units::math::abs(pose.Translation().Z() - - other.GetPose().Translation().Z()) < 1_in && - units::math::abs(pose.Rotation().X() - - other.GetPose().Rotation().X()) < 1_deg && - units::math::abs(pose.Rotation().Y() - - other.GetPose().Rotation().Y()) < 1_deg && - units::math::abs(pose.Rotation().Z() - - other.GetPose().Rotation().Z()) < 1_deg && + return wpi::units::math::abs(pose.Translation().X() - + other.GetPose().Translation().X()) < 1_in && + wpi::units::math::abs(pose.Translation().Y() - + other.GetPose().Translation().Y()) < 1_in && + wpi::units::math::abs(pose.Translation().Z() - + other.GetPose().Translation().Z()) < 1_in && + wpi::units::math::abs(pose.Rotation().X() - + other.GetPose().Rotation().X()) < 1_deg && + wpi::units::math::abs(pose.Rotation().Y() - + other.GetPose().Rotation().Y()) < 1_deg && + wpi::units::math::abs(pose.Rotation().Z() - + other.GetPose().Rotation().Z()) < 1_deg && model.GetIsPlanar() == other.GetModel().GetIsPlanar(); } @@ -170,7 +171,7 @@ class VisionTargetSim { int fiducialId; int objDetClassId; float objDetConf; - frc::Pose3d pose; + wpi::math::Pose3d pose; TargetModel model; }; } // namespace photon diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index 9e8f337462..5d776a3146 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -32,23 +32,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.ArrayList; import java.util.List; @@ -70,6 +53,23 @@ import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Quaternion; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.util.Nat; +import org.wpilib.math.util.Units; +import org.wpilib.util.runtime.RuntimeLoader; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; class LegacyPhotonPoseEstimatorTest { static AprilTagFieldLayout aprilTags; diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index e3087220e9..005ec0d0d7 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -26,13 +26,6 @@ import static org.junit.jupiter.api.Assertions.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTableInstance; import java.io.IOException; import java.util.List; import org.junit.jupiter.api.BeforeAll; @@ -45,6 +38,13 @@ import org.photonvision.jni.CombinedRuntimeLoader; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionTargetSim; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.MathUtil; +import org.wpilib.networktables.NetworkTableInstance; public class OpenCVTest { private static final double kTrlDelta = 0.005; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index c5ed3189b4..dfb0f2953a 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -31,15 +31,6 @@ import static org.photonvision.UnitTestUtils.waitForCondition; import static org.photonvision.UnitTestUtils.waitForSequenceNumber; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.NetworkTablesJNI; -import edu.wpi.first.util.RuntimeLoader; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.SimHooks; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.Arrays; import java.util.List; @@ -61,6 +52,15 @@ import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.targeting.PhotonPipelineMetadata; import org.photonvision.targeting.PhotonPipelineResult; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.NetworkTablesJNI; +import org.wpilib.simulation.SimHooks; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.DataLogManager; +import org.wpilib.system.Timer; +import org.wpilib.util.runtime.RuntimeLoader; @TestMethodOrder(MethodOrderer.OrderAnnotation.class) class PhotonCameraTest { @@ -260,7 +260,7 @@ public void testRestartingRobotAndCoproc( } if (i == robotStart || i == robotRestart) { - robotNt.startServer("networktables_random.json", "", 5940); + robotNt.startServer("networktables_random.json", "", "", 5940); } Thread.sleep(100); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index bfc169aa89..d880cd5bb3 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -29,21 +29,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.ArrayList; import java.util.List; @@ -63,6 +48,23 @@ import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Quaternion; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.util.Nat; +import org.wpilib.math.util.Units; +import org.wpilib.util.runtime.RuntimeLoader; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; class PhotonPoseEstimatorTest { static AprilTagFieldLayout aprilTags; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java index fbb632a91d..bf181c69dc 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java @@ -26,9 +26,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.*; +import org.wpilib.math.util.Units; class PhotonUtilTest { @Test diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 148286a17d..769fc2ce86 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -30,21 +30,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import static org.photonvision.UnitTestUtils.waitForSequenceNumber; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.RuntimeLoader; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.ArrayList; import java.util.List; @@ -64,6 +49,21 @@ import org.photonvision.simulation.VisionSystemSim; import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.util.runtime.RuntimeLoader; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.camera.OpenCvLoader; class VisionSystemSimTest { private static final double kRotDeltaDeg = 0.25; diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 0445be6b1e..9e3162cde2 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -48,13 +48,13 @@ #include "photon/targeting/PnpResult.h" WPI_IGNORE_DEPRECATED -static std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}}; +static std::vector tags = { + {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + wpi::math::Rotation3d())}}; -static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; +static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; static std::vector corners{ photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.}, @@ -69,24 +69,24 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -95,14 +95,14 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - frc::Transform3d{}); + wpi::math::Transform3d{}); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), .02); @@ -112,15 +112,15 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { } TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}, + std::vector tags = { + {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + wpi::math::Rotation3d())}, }; - auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft); + auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft); - std::vector> cameras; + std::vector> cameras; photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); @@ -130,24 +130,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -164,7 +164,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), .02); @@ -179,24 +179,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -207,7 +207,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, {}); estimator.SetReferencePose( - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -215,7 +215,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), .01); @@ -230,24 +230,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -258,7 +258,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, {}); estimator.SetLastPose( - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -266,29 +266,29 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; std::vector targetsThree{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.testResult = {photon::PhotonPipelineResult{ @@ -325,15 +325,15 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { &cameraOne, photon::SimCameraProperties::PERFECT_90DEG()); /* Compound Rolled + Pitched + Yaw */ - frc::Transform3d compoundTestTransform = frc::Transform3d( - -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg)); + wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d( + -12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg)); photon::PhotonPoseEstimator estimator( aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); /* real pose of the robot base to test against */ - frc::Pose3d realPose = - frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad)); + wpi::math::Pose3d realPose = + wpi::math::Pose3d(7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad)); photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), @@ -349,7 +349,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(units::unit_cast(realPose.X()), units::unit_cast(pose.X()), .01); @@ -359,12 +359,12 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { units::unit_cast(pose.Z()), .01); /* Straight on */ - frc::Transform3d straightOnTestTransform = - frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)); + wpi::math::Transform3d straightOnTestTransform = + wpi::math::Transform3d(0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)); estimator.SetRobotToCameraTransform(straightOnTestTransform); - realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, - frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); + realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m, + wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), targets); @@ -395,24 +395,24 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -429,7 +429,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.value().timestamp), .01); @@ -444,24 +444,24 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -520,8 +520,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { EXPECT_FALSE(estimatedPose); // Setting ReferencePose should also clear the cache - estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2), - units::meter_t(3), frc::Rotation3d())); + estimator.SetReferencePose(wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), + units::meter_t(3), wpi::math::Rotation3d())); for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); @@ -538,17 +538,17 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}}; cameraOne.test = true; @@ -557,14 +557,14 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - frc::Transform3d{}); + wpi::math::Transform3d{}); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), @@ -589,8 +589,8 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, frc::Transform3d()); + wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo), + photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d()); photon::PhotonPipelineResult result; auto estimate = estimator.Update(result); @@ -611,10 +611,10 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::TargetCorner{127.17118732489361, 313.81406314178633}, photon::TargetCorner{104.28543773760417, 309.6516557438994}}; - frc::Transform3d poseTransform( - frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m, + wpi::math::Transform3d poseTransform( + wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m, 0.48678786477534686_m), - frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333, + wpi::math::Rotation3d(wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333, -0.08413452932300695, 0.9130568172784148))); @@ -635,15 +635,15 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); const units::radian_t camPitch = 30_deg; - const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), - frc::Rotation3d(0_rad, -camPitch, 0_rad)}; + const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), + wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPoseEstimator estimator( - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), + wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo), photon::CONSTRAINED_SOLVEPNP, kRobotToCam); estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), - frc::Rotation2d()); + wpi::math::Rotation2d()); auto estimatedPose = estimator.Update(cameraOne.testResult[0], cameraMat, distortion, @@ -651,7 +651,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { ASSERT_TRUE(estimatedPose.has_value()); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(3.58, units::unit_cast(pose.X()), 0.01); EXPECT_NEAR(4.13, units::unit_cast(pose.Y()), 0.01); diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index e021299206..0f4b383ce4 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -26,14 +26,14 @@ #include #include -#include #include -#include #include #include -#include #include #include +#include +#include +#include TEST(TimeSyncProtocolTest, Smoketest) { using namespace wpi::tsp; @@ -60,10 +60,10 @@ TEST(TimeSyncProtocolTest, Smoketest) { } TEST(PhotonCameraTest, Alerts) { - using frc::SmartDashboard; + using wpi::SmartDashboard; // GIVEN a local-only NT instance - auto inst = nt::NetworkTableInstance::GetDefault(); + auto inst = wpi::nt::NetworkTableInstance::GetDefault(); inst.StopClient(); inst.StopServer(); inst.StartLocal(); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index de396a1987..e958ae79db 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -27,14 +27,14 @@ #include #include -#include -#include -#include -#include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "photon/PhotonCamera.h" #include "photon/dataflow/structures/Packet.h" @@ -46,13 +46,13 @@ #include "photon/targeting/PhotonTrackedTarget.h" #include "photon/targeting/PnpResult.h" -static std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}}; +static std::vector tags = { + {0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3), + wpi::units::meter_t(3), wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(wpi::units::meter_t(5), wpi::units::meter_t(5), + wpi::units::meter_t(5), wpi::math::Rotation3d())}}; -static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; +static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; static std::vector corners{ photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.}, @@ -67,57 +67,57 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{}); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.EstimateLowestAmbiguityPose(result); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), - .02); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(3, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR( + 11, wpi::units::unit_cast(estimatedPose.value().timestamp), .02); + EXPECT_NEAR(1, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(3, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}, + std::vector tags = { + {0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3), + wpi::units::meter_t(3), wpi::math::Rotation3d())}, + {1, wpi::math::Pose3d(wpi::units::meter_t(5), wpi::units::meter_t(5), + wpi::units::meter_t(5), wpi::math::Rotation3d())}, }; - auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft); + auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft); - std::vector> cameras; + std::vector> cameras; photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); @@ -127,24 +127,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; @@ -160,13 +160,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), - .02); - EXPECT_NEAR(4, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(0, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR( + 17, wpi::units::unit_cast(estimatedPose.value().timestamp), .02); + EXPECT_NEAR(4, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(4, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(0, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { @@ -175,30 +175,30 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -206,17 +206,17 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.EstimateClosestToReferencePose( result, - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), - .01); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(.9, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR( + 17, wpi::units::unit_cast(estimatedPose.value().timestamp), .01); + EXPECT_NEAR(1, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(1.1, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(.9, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { @@ -225,70 +225,70 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.EstimateClosestToReferencePose( - result, - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + result, wpi::math::Pose3d(1_m, 1_m, 1_m, + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; std::vector targetsThree{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.EstimateClosestToReferencePose(result, pose); @@ -297,11 +297,12 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { ASSERT_TRUE(estimatedPose); pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(21.0, units::unit_cast(estimatedPose.value().timestamp), + EXPECT_NEAR(21.0, + wpi::units::unit_cast(estimatedPose.value().timestamp), .01); - EXPECT_NEAR(.9, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(1, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR(.9, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(1.1, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(1, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { @@ -318,14 +319,14 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { &cameraOne, photon::SimCameraProperties::PERFECT_90DEG()); /* Compound Rolled + Pitched + Yaw */ - frc::Transform3d compoundTestTransform = frc::Transform3d( - -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg)); + wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d( + -12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg)); photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform); /* real pose of the robot base to test against */ - frc::Pose3d realPose = - frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad)); + wpi::math::Pose3d realPose = wpi::math::Pose3d( + 7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad)); photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), @@ -341,22 +342,22 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(units::unit_cast(realPose.X()), - units::unit_cast(pose.X()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Y()), - units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Z()), - units::unit_cast(pose.Z()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.X()), + wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.Y()), + wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.Z()), + wpi::units::unit_cast(pose.Z()), .01); /* Straight on */ - frc::Transform3d straightOnTestTransform = - frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)); + wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d( + 0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)); estimator.SetRobotToCameraTransform(straightOnTestTransform); - realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, - frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); + realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m, + wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), targets); @@ -373,12 +374,12 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { ASSERT_TRUE(estimatedPose); pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(units::unit_cast(realPose.X()), - units::unit_cast(pose.X()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Y()), - units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Z()), - units::unit_cast(pose.Z()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.X()), + wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.Y()), + wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(wpi::units::unit_cast(realPose.Z()), + wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, AverageBestPoses) { @@ -387,30 +388,30 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -420,13 +421,14 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.value().timestamp), + EXPECT_NEAR(15.0, + wpi::units::unit_cast(estimatedPose.value().timestamp), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2.15, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { @@ -435,25 +437,25 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), + wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}}; cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{}); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -464,14 +466,14 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { estimatedPose = estimator.EstimateLowestAmbiguityPose(result); } ASSERT_TRUE(estimatedPose); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy - EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), - .02); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(3, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2, units::unit_cast(pose.Z()), .01); + EXPECT_NEAR( + 11, wpi::units::unit_cast(estimatedPose.value().timestamp), .02); + EXPECT_NEAR(1, wpi::units::unit_cast(pose.X()), .01); + EXPECT_NEAR(3, wpi::units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2, wpi::units::unit_cast(pose.Z()), .01); } TEST(PhotonPoseEstimatorTest, CopyResult) { @@ -479,7 +481,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { auto testResult = photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; - testResult.SetReceiveTimestamp(units::second_t(11)); + testResult.SetReceiveTimestamp(wpi::units::second_t(11)); auto test2 = testResult; @@ -487,6 +489,17 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { test2.GetTimestamp().to(), 0.001); } +TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { + photon::PhotonPoseEstimator estimator( + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d()); + + photon::PhotonPipelineResult result; + auto estimate = estimator.Update(result); + EXPECT_FALSE(estimate.has_value()); +} + TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); auto distortion = Eigen::VectorXd::Zero(8); @@ -501,12 +514,12 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::TargetCorner{127.17118732489361, 313.81406314178633}, photon::TargetCorner{104.28543773760417, 309.6516557438994}}; - frc::Transform3d poseTransform( - frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m, - 0.48678786477534686_m), - frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333, - -0.08413452932300695, - 0.9130568172784148))); + wpi::math::Transform3d poseTransform( + wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m, + 0.48678786477534686_m), + wpi::math::Rotation3d( + wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333, + -0.08413452932300695, 0.9130568172784148))); std::vector targets{ photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform, @@ -522,21 +535,23 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { cameraOne.test = true; cameraOne.testResult = {result}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); + cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15)); - const units::radian_t camPitch = 30_deg; - const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), - frc::Rotation3d(0_rad, -camPitch, 0_rad)}; + const wpi::units::radian_t camPitch = 30_deg; + const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), + wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPoseEstimator estimator( - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), kRobotToCam); auto estimatedMultiTagPose = estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]); estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), - frc::Rotation2d()); + wpi::math::Rotation2d()); auto estimatedPose = estimator.EstimateConstrainedSolvepnpPose( cameraOne.testResult[0], cameraMat, distortion, @@ -544,11 +559,11 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { ASSERT_TRUE(estimatedPose.has_value()); - frc::Pose3d pose = estimatedPose.value().estimatedPose; + wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(3.58, units::unit_cast(pose.X()), 0.01); - EXPECT_NEAR(4.13, units::unit_cast(pose.Y()), 0.01); - EXPECT_NEAR(0.0, units::unit_cast(pose.Z()), 0.01); + EXPECT_NEAR(3.58, wpi::units::unit_cast(pose.X()), 0.01); + EXPECT_NEAR(4.13, wpi::units::unit_cast(pose.Y()), 0.01); + EXPECT_NEAR(0.0, wpi::units::unit_cast(pose.Z()), 0.01); EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy); } diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index abfbfd03b6..e68ece5e8e 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include "photon/PhotonUtils.h" #include "photon/estimation/VisionEstimation.h" @@ -38,7 +38,7 @@ WPI_IGNORE_DEPRECATED class VisionSystemSimTest : public ::testing::Test { void SetUp() override { - nt::NetworkTableInstance::GetDefault().StartServer(); + wpi::nt::NetworkTableInstance::GetDefault().StartServer(); photon::PhotonCamera::SetVersionCheckEnabled(false); } @@ -47,71 +47,73 @@ class VisionSystemSimTest : public ::testing::Test { class VisionSystemSimTestWithParamsTest : public VisionSystemSimTest, - public testing::WithParamInterface {}; + public testing::WithParamInterface {}; class VisionSystemSimTestDistanceParamsTest : public VisionSystemSimTest, public testing::WithParamInterface< - std::tuple> {}; + std::tuple> {}; TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { - frc::Pose3d targetPose{ - frc::Translation3d{15.98_m, 0_m, 2_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{15.98_m, 0_m, 2_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); // To the right, to the right - frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{-70_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); // To the right, to the right - robotPose = - frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{-95_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); // To the left, to the left - robotPose = - frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{90_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); // To the left, to the left - robotPose = - frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{65_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); // Now kick, now kick - robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); // Now kick, now kick - robotPose = - frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m}, + wpi::math::Rotation2d{-5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); // Now walk it by yourself - robotPose = - frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m}, + wpi::math::Rotation2d{-179_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); // Now walk it by yourself visionSysSim.AdjustCamera( - &cameraSim, - frc::Transform3d{ - frc::Translation3d{}, - frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + &cameraSim, wpi::math::Transform3d{ + wpi::math::Translation3d{}, + wpi::math::Rotation3d{ + 0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}}); visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); } @@ -140,121 +142,129 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) { } TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { - frc::Pose3d targetPose{ - frc::Translation3d{15.98_m, 0_m, 1_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{15.98_m, 0_m, 1_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); - frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); visionSysSim.AdjustCamera( - &cameraSim, - frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 5000_m}, - frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + &cameraSim, wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 5000_m}, + wpi::math::Rotation3d{ + 0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}}); visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); } TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { - frc::Pose3d targetPose{ - frc::Translation3d{15.98_m, 0_m, 2_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{15.98_m, 0_m, 2_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; - frc::Transform3d robotToCamera{ - frc::Translation3d{0_m, 0_m, 1_m}, - frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + wpi::math::Transform3d robotToCamera{ + wpi::math::Translation3d{0_m, 0_m, 1_m}, + wpi::math::Rotation3d{0_rad, wpi::units::radian_t{-std::numbers::pi / 4}, + 0_rad}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, robotToCamera); - cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + cameraSim.prop.SetCalibration(1234, 1234, wpi::math::Rotation2d{80_deg}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); - frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, - frc::Rotation2d{5_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{13.98_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); - robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); } TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { - frc::Pose3d targetPose{ - frc::Translation3d{15.98_m, 0_m, 1_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{15.98_m, 0_m, 1_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(20.0); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); - frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{12_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); - robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); } TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { - frc::Pose3d targetPose{ - frc::Translation3d{15.98_m, 0_m, 1_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{15.98_m, 0_m, 1_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(1.0); cameraSim.SetMaxSightRange(10_m); visionSysSim.AddVisionTargets( {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); - frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_TRUE(camera.GetLatestResult().HasTargets()); - robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.Update(robotPose); ASSERT_FALSE(camera.GetLatestResult().HasTargets()); } TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { - const frc::Pose3d targetPose{ + const wpi::math::Pose3d targetPose{ {15.98_m, 0_m, 0_m}, - frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + wpi::math::Rotation3d{0_deg, 0_deg, + wpi::units::radian_t{3 * std::numbers::pi / 4}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(0.0); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) - frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, - frc::Rotation2d{GetParam()}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m}, + wpi::math::Rotation2d{GetParam()}}; visionSysSim.Update(robotPose); const auto result = camera.GetLatestResult(); @@ -263,26 +273,28 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { } TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { - const frc::Pose3d targetPose{ + const wpi::math::Pose3d targetPose{ {15.98_m, 0_m, 0_m}, - frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; - frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + wpi::math::Rotation3d{0_deg, 0_deg, + wpi::units::radian_t{3 * std::numbers::pi / 4}}}; + wpi::math::Pose2d robotPose{{10_m, 0_m}, + wpi::math::Rotation2d{GetParam() * -1.0}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{120_deg}); cameraSim.SetMinTargetAreaPixels(0.0); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); - robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, - frc::Rotation2d{-1 * GetParam()}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{10_m, 0_m}, + wpi::math::Rotation2d{-1 * GetParam()}}; visionSysSim.AdjustCamera( &cameraSim, - frc::Transform3d{ - frc::Translation3d{}, - frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + wpi::math::Transform3d{ + wpi::math::Translation3d{}, + wpi::math::Rotation3d{0_rad, wpi::units::degree_t{GetParam()}, 0_rad}}); visionSysSim.Update(robotPose); const auto result = camera.GetLatestResult(); @@ -295,24 +307,27 @@ INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, -2_deg, 5_deg, 7_deg, 10.23_deg)); TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { - units::foot_t distParam; - units::degree_t pitchParam; - units::foot_t heightParam; + wpi::units::foot_t distParam; + wpi::units::degree_t pitchParam; + wpi::units::foot_t heightParam; std::tie(distParam, pitchParam, heightParam) = GetParam(); - const frc::Pose3d targetPose{ + const wpi::math::Pose3d targetPose{ {15.98_m, 0_m, 1_m}, - frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; - frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; - frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, - frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + wpi::math::Rotation3d{0_deg, 0_deg, + wpi::units::radian_t{std::numbers::pi * 0.98}}}; + wpi::math::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, + wpi::math::Rotation3d{}}; + wpi::math::Transform3d robotToCamera{ + wpi::math::Translation3d{0_m, 0_m, heightParam}, + wpi::math::Rotation3d{0_deg, pitchParam, 0_deg}}; photon::VisionSystemSim visionSysSim{ "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{160_deg}); cameraSim.SetMinTargetAreaPixels(0.0); visionSysSim.AdjustCamera(&cameraSim, robotToCamera); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ @@ -325,11 +340,11 @@ TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { ASSERT_NEAR(0.0, target.GetYaw(), 0.5); - units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + wpi::units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( robotToCamera.Z(), targetPose.Z(), -pitchParam, - units::degree_t{target.GetPitch()}); + wpi::units::degree_t{target.GetPitch()}); ASSERT_NEAR(dist.to(), - distParam.convert().to(), 0.25); + distParam.convert().to(), 0.25); } INSTANTIATE_TEST_SUITE_P( @@ -353,69 +368,70 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft))); TEST_F(VisionSystemSimTest, TestMultipleTargets) { - frc::Pose3d targetPoseL{ - frc::Translation3d{15.98_m, 2_m, 0_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - frc::Pose3d targetPoseC{ - frc::Translation3d{15.98_m, 0_m, 0_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; - frc::Pose3d targetPoseR{ - frc::Translation3d{15.98_m, -2_m, 0_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPoseL{ + wpi::math::Translation3d{15.98_m, 2_m, 0_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPoseC{ + wpi::math::Translation3d{15.98_m, 0_m, 0_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPoseR{ + wpi::math::Translation3d{15.98_m, -2_m, 0_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(20.0); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseL.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + targetPoseL.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 1}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseC.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + targetPoseC.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 2}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseR.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + targetPoseR.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 3}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseL.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + targetPoseL.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 4}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseC.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + targetPoseC.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 5}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseR.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + targetPoseR.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 6}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseL.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + targetPoseL.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 7}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseC.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + targetPoseC.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 8}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseL.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + targetPoseL.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 9}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseR.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + targetPoseR.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 10}}); visionSysSim.AddVisionTargets({photon::VisionTargetSim{ - targetPoseL.TransformBy(frc::Transform3d{ - frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + targetPoseL.TransformBy(wpi::math::Transform3d{ + wpi::math::Translation3d{0_m, 0_m, 0.25_m}, wpi::math::Rotation3d{}}), photon::kAprilTag16h5, 11}}); - frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{6_m, 0_m}, + wpi::math::Rotation2d{.25_deg}}; visionSysSim.Update(robotPose); photon::PhotonPipelineResult res = camera.GetLatestResult(); ASSERT_TRUE(res.HasTargets()); @@ -427,27 +443,31 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg}); cameraSim.SetMinTargetAreaPixels(20.0); - std::vector tagList; - tagList.emplace_back(frc::AprilTag{ - 0, frc::Pose3d{12_m, 3_m, 1_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - tagList.emplace_back(frc::AprilTag{ - 1, frc::Pose3d{12_m, 1_m, -1_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - tagList.emplace_back(frc::AprilTag{ - 2, frc::Pose3d{11_m, 0_m, 2_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - units::meter_t fieldLength{54}; - units::meter_t fieldWidth{27}; - frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; - frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + std::vector tagList; + tagList.emplace_back(wpi::apriltag::AprilTag{ + 0, + wpi::math::Pose3d{12_m, 3_m, 1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(wpi::apriltag::AprilTag{ + 1, + wpi::math::Pose3d{12_m, 1_m, -1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(wpi::apriltag::AprilTag{ + 2, + wpi::math::Pose3d{11_m, 0_m, 2_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + wpi::units::meter_t fieldLength{54}; + wpi::units::meter_t fieldWidth{27}; + wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m}, + wpi::math::Rotation2d{5_deg}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); visionSysSim.Update(robotPose); @@ -464,11 +484,11 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { auto results = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets, layout, photon::kAprilTag16h5); ASSERT_TRUE(results); - frc::Pose3d pose = frc::Pose3d{} + results->best; + wpi::math::Pose3d pose = wpi::math::Pose3d{} + results->best; ASSERT_NEAR(5, pose.X().to(), 0.01); ASSERT_NEAR(1, pose.Y().to(), 0.01); ASSERT_NEAR(0, pose.Z().to(), 0.01); - ASSERT_NEAR(units::degree_t{5}.convert().to(), + ASSERT_NEAR(wpi::units::degree_t{5}.convert().to(), pose.Rotation().Z().to(), 0.01); visionSysSim.AddVisionTargets( @@ -486,42 +506,47 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { auto results2 = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); ASSERT_TRUE(results2); - frc::Pose3d pose2 = frc::Pose3d{} + results2->best; + wpi::math::Pose3d pose2 = wpi::math::Pose3d{} + results2->best; ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); ASSERT_NEAR(0, pose2.Z().to(), 0.01); - ASSERT_NEAR(units::degree_t{5}.convert().to(), + ASSERT_NEAR(wpi::units::degree_t{5}.convert().to(), pose2.Rotation().Z().to(), 0.01); } TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { - frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in}, - frc::Rotation3d{0_deg, -30_deg, 25.5_deg}}; + wpi::math::Transform3d robotToCamera{ + wpi::math::Translation3d{6_in, 6_in, 6_in}, + wpi::math::Rotation3d{0_deg, -30_deg, 25.5_deg}}; photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"cameraRotated"}; photon::PhotonCameraSim cameraSim{&camera}; visionSysSim.AddCamera(&cameraSim, robotToCamera); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg}); cameraSim.SetMinTargetAreaPixels(20.0); - std::vector tagList; - tagList.emplace_back(frc::AprilTag{ - 0, frc::Pose3d{12_m, 3_m, 1_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - tagList.emplace_back(frc::AprilTag{ - 1, frc::Pose3d{12_m, 1_m, -1_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - tagList.emplace_back(frc::AprilTag{ - 2, frc::Pose3d{11_m, 0_m, 2_m, - frc::Rotation3d{0_rad, 0_rad, - units::radian_t{std::numbers::pi}}}}); - units::meter_t fieldLength{54}; - units::meter_t fieldWidth{27}; - frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; - frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}}; + std::vector tagList; + tagList.emplace_back(wpi::apriltag::AprilTag{ + 0, + wpi::math::Pose3d{12_m, 3_m, 1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(wpi::apriltag::AprilTag{ + 1, + wpi::math::Pose3d{12_m, 1_m, -1_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(wpi::apriltag::AprilTag{ + 2, + wpi::math::Pose3d{11_m, 0_m, 2_m, + wpi::math::Rotation3d{ + 0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}}); + wpi::units::meter_t fieldLength{54}; + wpi::units::meter_t fieldWidth{27}; + wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m}, + wpi::math::Rotation2d{-5_deg}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}}); visionSysSim.Update(robotPose); @@ -542,12 +567,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { auto results = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets, layout, photon::kAprilTag36h11); ASSERT_TRUE(results); - frc::Pose3d pose = frc::Pose3d{} + results->best; + wpi::math::Pose3d pose = wpi::math::Pose3d{} + results->best; pose = pose.TransformBy(robotToCamera.Inverse()); ASSERT_NEAR(5, pose.X().to(), 0.01); ASSERT_NEAR(1, pose.Y().to(), 0.01); ASSERT_NEAR(0, pose.Z().to(), 0.01); - ASSERT_NEAR(units::degree_t{-5}.convert().to(), + ASSERT_NEAR(wpi::units::degree_t{-5}.convert().to(), pose.Rotation().Z().to(), 0.01); visionSysSim.AddVisionTargets( @@ -565,12 +590,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { auto results2 = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets2, layout, photon::kAprilTag36h11); ASSERT_TRUE(results2); - frc::Pose3d pose2 = frc::Pose3d{} + results2->best; + wpi::math::Pose3d pose2 = wpi::math::Pose3d{} + results2->best; pose2 = pose2.TransformBy(robotToCamera.Inverse()); ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); ASSERT_NEAR(0, pose2.Z().to(), 0.01); - ASSERT_NEAR(units::degree_t{-5}.convert().to(), + ASSERT_NEAR(wpi::units::degree_t{-5}.convert().to(), pose2.Rotation().Z().to(), 0.01); } @@ -578,24 +603,25 @@ TEST_F(VisionSystemSimTest, TestTagAmbiguity) { photon::VisionSystemSim visionSysSim{"Test"}; photon::PhotonCamera camera{"camera"}; photon::PhotonCameraSim cameraSim{&camera}; - visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); - cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg}); cameraSim.SetMinTargetAreaPixels(20.0); - frc::Pose3d targetPose{ - frc::Translation3d{2_m, 0_m, 0_m}, - frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + wpi::math::Pose3d targetPose{ + wpi::math::Translation3d{2_m, 0_m, 0_m}, + wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}; visionSysSim.AddVisionTargets( {photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}}); - frc::Pose2d robotPose{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{0_deg}}; + wpi::math::Pose2d robotPose{wpi::math::Translation2d{0_m, 0_m}, + wpi::math::Rotation2d{0_deg}}; visionSysSim.Update(robotPose); double ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity(); ASSERT_TRUE(ambiguity > 0.5); - robotPose = - frc::Pose2d{frc::Translation2d{-2_m, -2_m}, frc::Rotation2d{30_deg}}; + robotPose = wpi::math::Pose2d{wpi::math::Translation2d{-2_m, -2_m}, + wpi::math::Rotation2d{30_deg}}; visionSysSim.Update(robotPose); ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity(); ASSERT_TRUE(0 < ambiguity && ambiguity < 0.2); diff --git a/photon-lib/src/test/native/cpp/main.cpp b/photon-lib/src/test/native/cpp/main.cpp index 351c8d3378..6b8949b2f2 100644 --- a/photon-lib/src/test/native/cpp/main.cpp +++ b/photon-lib/src/test/native/cpp/main.cpp @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include +#include #include "gtest/gtest.h" diff --git a/photon-serde/generate_messages.py b/photon-serde/generate_messages.py index 2efca52812..8013b0f120 100644 --- a/photon-serde/generate_messages.py +++ b/photon-serde/generate_messages.py @@ -93,7 +93,7 @@ def get_qualified_cpp_name( """ Get the full name of the type encoded. Eg: std::optional - std::array + std::array """ if get_shimmed_filter(message_db)(field["type"]): diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index 340e771807..40cbc1436b 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -14,11 +14,11 @@ shimmed: True java_decode_shim: PacketUtils.unpackTransform3d java_encode_shim: PacketUtils.packTransform3d - cpp_type: frc::Transform3d - cpp_include: "" + cpp_type: wpi::math::Transform3d + cpp_include: "" python_decode_shim: decodeTransform python_encode_shim: encodeTransform - java_import: edu.wpi.first.math.geometry.Transform3d + java_import: org.wpilib.math.geometry.Transform3d # shim since we expect fields to at least exist fields: [] diff --git a/photon-serde/templates/Message.java.jinja b/photon-serde/templates/Message.java.jinja index b1e27b68e9..75983e6a51 100644 --- a/photon-serde/templates/Message.java.jinja +++ b/photon-serde/templates/Message.java.jinja @@ -34,7 +34,7 @@ import org.photonvision.utils.PacketUtils; import org.photonvision.targeting.*; // WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; {% for type in nested_wpilib_types -%} import {{ get_message_by_name(type).java_import }}; {%- if not loop.last %},{% endif -%} diff --git a/photon-serde/templates/ThingSerde.h.jinja b/photon-serde/templates/ThingSerde.h.jinja index 8e5ac7eb27..8d9597a1a0 100644 --- a/photon-serde/templates/ThingSerde.h.jinja +++ b/photon-serde/templates/ThingSerde.h.jinja @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" diff --git a/photon-server/build.gradle b/photon-server/build.gradle index 6269bb8efd..77a137a28f 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -75,9 +75,9 @@ run { } } -import edu.wpi.first.deployutils.deploy.artifact.* -import edu.wpi.first.deployutils.deploy.target.RemoteTarget -import edu.wpi.first.deployutils.deploy.target.location.SshDeployLocation +import org.wpilib.deployutils.deploy.artifact.* +import org.wpilib.deployutils.deploy.target.RemoteTarget +import org.wpilib.deployutils.deploy.target.location.SshDeployLocation deploy { targets { pi(RemoteTarget) { diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index b07538a587..9c353515eb 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -17,8 +17,6 @@ package org.photonvision; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Rotation2d; import java.io.IOException; import java.nio.file.Path; import java.util.ArrayList; @@ -56,6 +54,7 @@ import org.photonvision.vision.pipeline.PipelineProfiler; import org.photonvision.vision.processes.VisionSourceManager; import org.photonvision.vision.target.TargetModel; +import org.wpilib.hardware.hal.HAL; public class Main { public static final int DEFAULT_WEBPORT = 5800; diff --git a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java index c42b876d96..8d15c8d555 100644 --- a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java @@ -20,7 +20,6 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.core.type.TypeReference; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.Pair; import io.javalin.websocket.WsBinaryMessageContext; import io.javalin.websocket.WsCloseContext; import io.javalin.websocket.WsConnectContext; @@ -42,6 +41,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.pipeline.PipelineType; +import org.wpilib.math.util.Pair; @SuppressWarnings("rawtypes") public class DataSocketHandler { diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index ed132739a3..6fcab1a46a 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -5,9 +5,9 @@ ext { apply plugin: 'cpp' apply plugin: 'c' apply plugin: 'google-test-test-suite' -apply plugin: 'edu.wpi.first.NativeUtils' +apply plugin: 'org.wpilib.NativeUtils' apply plugin: 'org.photonvision.tools.WpilibTools' -apply plugin: 'edu.wpi.first.GradleJni' +apply plugin: 'org.wpilib.GradleJni' ext.licenseFile = file("$rootDir/LICENSE") apply from: "${rootDir}/shared/config.gradle" @@ -67,7 +67,6 @@ model { enableCheckTask project.hasProperty('doJniCheck') javaCompileTasks << compileJava jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) - jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) sources { @@ -120,7 +119,7 @@ model { } } - nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") + nativeUtils.useRequiredLibrary(it, "wpilib_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "datalog_shared") nativeUtils.useRequiredLibrary(it, "apriltag_shared") diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 5f9134fda8..17c3aba09f 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -86,7 +86,7 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct,PhotonTrackedTarget.photonStruct + PhotonTrackedTarget.photonStruct,MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct }; } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java index 7a1e5363ab..ed08a8b58c 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java @@ -35,7 +35,7 @@ // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PhotonTrackedTarget diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java index 4b61e5cbd4..73a2d65cb0 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java @@ -35,7 +35,7 @@ // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PnpResult diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonTrackedTargetSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonTrackedTargetSerde.cpp index a6b542322b..d1dbbf0c1b 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonTrackedTargetSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonTrackedTargetSerde.cpp @@ -38,8 +38,8 @@ void StructType::Pack(Packet& packet, const PhotonTrackedTarget& value) { packet.Pack(value.fiducialId); packet.Pack(value.objDetectId); packet.Pack(value.objDetectConf); - packet.Pack(value.bestCameraToTarget); - packet.Pack(value.altCameraToTarget); + packet.Pack(value.bestCameraToTarget); + packet.Pack(value.altCameraToTarget); packet.Pack(value.poseAmbiguity); packet.Pack>(value.minAreaRectCorners); packet.Pack>(value.detectedCorners); @@ -54,8 +54,8 @@ PhotonTrackedTarget StructType::Unpack(Packet& packet) { .fiducialId = packet.Unpack(), .objDetectId = packet.Unpack(), .objDetectConf = packet.Unpack(), - .bestCameraToTarget = packet.Unpack(), - .altCameraToTarget = packet.Unpack(), + .bestCameraToTarget = packet.Unpack(), + .altCameraToTarget = packet.Unpack(), .poseAmbiguity = packet.Unpack(), .minAreaRectCorners = packet.Unpack>(), .detectedCorners = packet.Unpack>(), diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PnpResultSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PnpResultSerde.cpp index 05355c12b0..0fc4a5882f 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PnpResultSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PnpResultSerde.cpp @@ -31,8 +31,8 @@ namespace photon { using StructType = SerdeType; void StructType::Pack(Packet& packet, const PnpResult& value) { - packet.Pack(value.best); - packet.Pack(value.alt); + packet.Pack(value.best); + packet.Pack(value.alt); packet.Pack(value.bestReprojErr); packet.Pack(value.altReprojErr); packet.Pack(value.ambiguity); @@ -40,8 +40,8 @@ void StructType::Pack(Packet& packet, const PnpResult& value) { PnpResult StructType::Unpack(Packet& packet) { return PnpResult{ PnpResult_PhotonStruct{ - .best = packet.Unpack(), - .alt = packet.Unpack(), + .best = packet.Unpack(), + .alt = packet.Unpack(), .bestReprojErr = packet.Unpack(), .altReprojErr = packet.Unpack(), .ambiguity = packet.Unpack(), diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h index 1e79462c19..362b87c0fe 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h index b4afd24b7a..3cb4a9b24c 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 836e347ca1..b64adbacf1 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h index c7ce79bb83..468be90211 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" @@ -34,9 +34,9 @@ // Includes for dependant types #include "photon/targeting/TargetCorner.h" -#include #include #include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h index d01eed23de..8dee1e8a49 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h @@ -26,15 +26,15 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" #include "photon/targeting/PnpResult.h" // Includes for dependant types -#include #include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h index cd587ae30e..64ad4c27a7 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h @@ -26,7 +26,7 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY -#include +#include // Include myself #include "photon/dataflow/structures/Packet.h" diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h index 2877c5d582..40586156d8 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h @@ -28,9 +28,9 @@ // Includes for dependant types #include "photon/targeting/TargetCorner.h" -#include #include #include +#include namespace photon { @@ -43,8 +43,8 @@ struct PhotonTrackedTarget_PhotonStruct { int32_t fiducialId; int32_t objDetectId; float objDetectConf; - frc::Transform3d bestCameraToTarget; - frc::Transform3d altCameraToTarget; + wpi::math::Transform3d bestCameraToTarget; + wpi::math::Transform3d altCameraToTarget; double poseAmbiguity; std::vector minAreaRectCorners; std::vector detectedCorners; diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h index 7cdbd70177..7da4c5ad0a 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h @@ -27,15 +27,15 @@ // THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY // Includes for dependant types -#include #include +#include namespace photon { struct PnpResult_PhotonStruct { - frc::Transform3d best; - frc::Transform3d alt; + wpi::math::Transform3d best; + wpi::math::Transform3d alt; double bestReprojErr; double altReprojErr; double ambiguity; diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java index 3cd0abca12..b77dc37ed0 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java @@ -17,7 +17,7 @@ package org.photonvision.common.dataflow.structures; -import edu.wpi.first.util.struct.Struct; +import org.wpilib.util.struct.Struct; @SuppressWarnings("doclint") public interface PacketSerde { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 5b3d50d0fc..ff450adf2d 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -17,20 +17,20 @@ package org.photonvision.common.networktables; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.BooleanPublisher; -import edu.wpi.first.networktables.BooleanSubscriber; -import edu.wpi.first.networktables.BooleanTopic; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.IntegerTopic; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.ProtobufPublisher; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StructPublisher; import org.photonvision.targeting.PhotonPipelineResult; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.networktables.BooleanPublisher; +import org.wpilib.networktables.BooleanSubscriber; +import org.wpilib.networktables.BooleanTopic; +import org.wpilib.networktables.DoubleArrayPublisher; +import org.wpilib.networktables.DoublePublisher; +import org.wpilib.networktables.IntegerPublisher; +import org.wpilib.networktables.IntegerSubscriber; +import org.wpilib.networktables.IntegerTopic; +import org.wpilib.networktables.NetworkTable; +import org.wpilib.networktables.ProtobufPublisher; +import org.wpilib.networktables.PubSubOption; +import org.wpilib.networktables.StructPublisher; /** * This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java index 1fad03fc44..db0a002717 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java @@ -19,11 +19,11 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.networktables.RawPublisher; import java.util.HashSet; import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.wpilib.networktables.RawPublisher; @SuppressWarnings("doclint") public class PacketPublisher implements AutoCloseable { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java index f6b7ecb8d0..315392c150 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java @@ -17,11 +17,11 @@ package org.photonvision.common.networktables; -import edu.wpi.first.networktables.RawSubscriber; import java.util.ArrayList; import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.wpilib.networktables.RawSubscriber; @SuppressWarnings("doclint") public class PacketSubscriber implements AutoCloseable { diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java index 7b64cdf4ef..c6d345685e 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java @@ -17,9 +17,9 @@ package org.photonvision.estimation; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform3d; /** Holds various helper geometries describing the relation between camera and target. */ @SuppressWarnings("doclint") diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 4b239f67d5..857f48c8a5 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -17,16 +17,6 @@ package org.photonvision.estimation; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.Num; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.*; import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -48,6 +38,17 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.*; +import org.wpilib.math.util.Nat; +import org.wpilib.math.util.Num; +import org.wpilib.vision.camera.OpenCvLoader; /** * A set of various utility functions for getting non-OpenCV data into an OpenCV-compatible format, diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index c3e93c8ebb..884b35276b 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -17,12 +17,12 @@ package org.photonvision.estimation; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import java.util.stream.Collectors; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; /** * Represents a transformation that first rotates a pose around the origin, and then translates it. diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index 0435a62bd5..7e10bb7f78 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -17,14 +17,14 @@ package org.photonvision.estimation; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; import java.util.stream.Collectors; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; /** Describes the 3d model of a target. */ public class TargetModel { diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 98229ac6ad..4702086ef9 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -17,18 +17,6 @@ package org.photonvision.estimation; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.*; import java.util.ArrayList; import java.util.List; import java.util.Objects; @@ -43,6 +31,18 @@ import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.numbers.*; +import org.wpilib.math.util.Nat; +import org.wpilib.vision.apriltag.AprilTag; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.camera.OpenCvLoader; public class VisionEstimation { private VisionEstimation() {} diff --git a/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java b/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java index ccce0e79b4..75acf4c2e9 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/CscoreExtras.java @@ -17,8 +17,8 @@ package org.photonvision.jni; -import edu.wpi.first.util.RawFrame; -import edu.wpi.first.util.TimestampSource; +import org.wpilib.util.RawFrame; +import org.wpilib.util.TimestampSource; public class CscoreExtras { /** @@ -30,7 +30,7 @@ public class CscoreExtras { * CvSource provides a new frame. * * @param sink Sink handle. - * @param framePtr Pointer to a wpi::RawFrame. + * @param framePtr Pointer to a wpi::util::RawFrame. * @param timeout Timeout in seconds. * @param lastFrameTime Timestamp of the last frame - used to compare new frames against. * @return Frame time, in uS, of the incoming frame. diff --git a/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java b/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java index 50f855464a..8c1301aef9 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/LibraryLoader.java @@ -17,17 +17,17 @@ package org.photonvision.jni; -import edu.wpi.first.apriltag.jni.AprilTagJNI; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.datalog.DataLogJNI; -import edu.wpi.first.hal.JNIWrapper; -import edu.wpi.first.math.jni.WPIMathJNI; -import edu.wpi.first.net.WPINetJNI; -import edu.wpi.first.networktables.NetworkTablesJNI; -import edu.wpi.first.util.WPIUtilJNI; import java.io.IOException; import org.opencv.core.Core; +import org.wpilib.datalog.DataLogJNI; +import org.wpilib.hardware.hal.JNIWrapper; +import org.wpilib.math.jni.WPIMathJNI; +import org.wpilib.net.WPINetJNI; +import org.wpilib.networktables.NetworkTablesJNI; +import org.wpilib.util.WPIUtilJNI; +import org.wpilib.vision.apriltag.jni.AprilTagJNI; +import org.wpilib.vision.camera.CameraServerJNI; +import org.wpilib.vision.camera.OpenCvLoader; public class LibraryLoader { private static boolean hasWpiLoaded = false; diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java index 2151797cd3..cc2f94b752 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java @@ -17,11 +17,12 @@ package org.photonvision.jni; -import edu.wpi.first.networktables.NetworkTablesJNI; +import org.wpilib.networktables.NetworkTablesJNI; /** - * Send ping-pongs to estimate server time, relative to nt::Now. The underlying implementation does - * technically allow us to provide a different source, but all photon code assumes nt::Now is used + * Send ping-pongs to estimate server time, relative to wpi::nt::Now. The underlying implementation + * does technically allow us to provide a different source, but all photon code assumes wpi::nt::Now + * is used */ public class TimeSyncClient { public static class PingMetadata { @@ -112,8 +113,8 @@ public void stop() { } /** - * This offset, when added to the current value of nt::now(), yields the timestamp in the timebase - * of the TSP Server + * This offset, when added to the current value of wpi::nt::now(), yields the timestamp in the + * timebase of the TSP Server * * @return */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index da105ba906..e7b3ed9a40 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -17,12 +17,12 @@ package org.photonvision.targeting; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.List; import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.struct.MultiTargetPNPResultSerde; import org.photonvision.targeting.proto.MultiTargetPNPResultProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.util.protobuf.ProtobufSerializable; public class MultiTargetPNPResult implements ProtobufSerializable, PhotonStructSerializable { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java index 9c68468291..e5687ca427 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java @@ -23,7 +23,7 @@ public class PhotonPipelineMetadata implements PhotonStructSerializable { // Image capture and NT publish timestamp, in microseconds - // The timebase is nt::Now on the time sync server + // The timebase is wpi::nt::Now on the time sync server public long captureTimestampMicros; public long publishTimestampMicros; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index a599c9a5c8..7945276d99 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -17,7 +17,6 @@ package org.photonvision.targeting; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -25,6 +24,7 @@ import org.photonvision.struct.PhotonPipelineResultSerde; import org.photonvision.targeting.proto.PhotonPipelineResultProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.util.protobuf.ProtobufSerializable; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult @@ -168,9 +168,9 @@ public Optional getMultiTagResult() { } /** - * Returns the estimated time the frame was taken, in the Time Sync Server's time base (nt::Now). - * This is calculated using the estimated offset between Time Sync Server time and local time. The - * robot shall run a server, so the offset shall be 0. + * Returns the estimated time the frame was taken, in the Time Sync Server's time base + * (wpi::nt::Now). This is calculated using the estimated offset between Time Sync Server time and + * local time. The robot shall run a server, so the offset shall be 0. * * @return The timestamp in seconds */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 58b4c8d125..28b143d776 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -17,13 +17,13 @@ package org.photonvision.targeting; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.List; import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.struct.PhotonTrackedTargetSerde; import org.photonvision.targeting.proto.PhotonTrackedTargetProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.util.protobuf.ProtobufSerializable; /** Information about a detected target. */ public class PhotonTrackedTarget diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java index 6be6ff44d2..f28524ea8f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java @@ -17,12 +17,12 @@ package org.photonvision.targeting; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.struct.PnpResultSerde; import org.photonvision.targeting.proto.PNPResultProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.util.protobuf.ProtobufSerializable; /** * The best estimated transformation from solvePnP, and possibly an alternate transformation diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 0ca1222554..319d82a66c 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -17,12 +17,12 @@ package org.photonvision.targeting; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.Objects; import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.struct.TargetCornerSerde; import org.photonvision.targeting.proto.TargetCornerProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.util.protobuf.ProtobufSerializable; /** * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java index 934dccc987..e3e8b48296 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java @@ -17,11 +17,11 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PnpResult; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedInt; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java index 9bb7022d45..dbe185173b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java @@ -17,10 +17,10 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.Protobuf; import org.photonvision.proto.Photon.ProtobufPNPResult; import org.photonvision.targeting.PnpResult; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; public class PNPResultProto implements Protobuf { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 97bf80e6cc..e1f2039c0a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -17,12 +17,12 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.util.protobuf.Protobuf; import java.util.Optional; import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; public class PhotonPipelineResultProto diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java index 8bc68ad326..477721c758 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java @@ -17,13 +17,13 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java index 1067320272..22db47f9f7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java @@ -17,11 +17,11 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; import org.photonvision.proto.Photon.ProtobufTargetCorner; import org.photonvision.targeting.TargetCorner; +import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java index 187cfc0ef1..501d753a24 100644 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -17,8 +17,8 @@ package org.photonvision.utils; -import edu.wpi.first.math.geometry.*; import org.photonvision.common.dataflow.structures.Packet; +import org.wpilib.math.geometry.*; @SuppressWarnings("doclint") public class PacketUtils { diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp index 15e4c343a2..9df7724813 100644 --- a/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp +++ b/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp @@ -22,12 +22,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include "ntcore_cpp.h" +#include "wpi/nt/ntcore_cpp.hpp" static void ClientLoggerFunc(unsigned int level, const char* file, unsigned int line, const char* msg) { @@ -62,8 +62,8 @@ void wpi::tsp::TimeSyncClient::UpdateStatistics(uint64_t pong_local_time, auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs); - // wpi::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS", - // rtt2, + // wpi::util::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} + // uS", rtt2, // serverTimeOffsetUs, filtered); { @@ -76,7 +76,7 @@ void wpi::tsp::TimeSyncClient::UpdateStatistics(uint64_t pong_local_time, } void wpi::tsp::TimeSyncClient::Tick() { - // wpi::println("wpi::tsp::TimeSyncClient::Tick"); + // wpi::util::println("wpi::tsp::TimeSyncClient::Tick"); // Regardless of if we've gotten a pong back yet, we'll ping again. this is // pretty naive but should be "fine" for now? @@ -84,15 +84,16 @@ void wpi::tsp::TimeSyncClient::Tick() { TspPing ping{.version = 1, .message_id = 1, .client_time = ping_local_time}; - wpi::SmallVector::GetSize()> pingData( - wpi::Struct::GetSize()); - wpi::PackStruct(pingData, ping); + wpi::util::SmallVector::GetSize()> + pingData(wpi::util::Struct::GetSize()); + wpi::util::PackStruct(pingData, ping); // Wrap our buffer - pingData should free itself - wpi::uv::Buffer pingBuf{pingData}; - int sent = m_udp->TrySend(wpi::SmallVector{pingBuf}); + wpi::net::uv::Buffer pingBuf{pingData}; + int sent = + m_udp->TrySend(wpi::util::SmallVector{pingBuf}); - if (static_cast(sent) != wpi::Struct::GetSize()) { + if (static_cast(sent) != wpi::util::Struct::GetSize()) { WPI_ERROR(m_logger, "Didn't send the whole ping out? sent {} bytes", sent); return; } @@ -105,29 +106,29 @@ void wpi::tsp::TimeSyncClient::Tick() { m_lastPing = ping; } -void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes, +void wpi::tsp::TimeSyncClient::UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, const sockaddr& sender, unsigned flags) { uint64_t pong_local_time = m_timeProvider(); - if (static_cast(nbytes) != wpi::Struct::GetSize()) { + if (static_cast(nbytes) != wpi::util::Struct::GetSize()) { WPI_ERROR(m_logger, "Got {} bytes for pong?", nbytes); return; } TspPong pong{ - wpi::UnpackStruct(buf.bytes()), + wpi::util::UnpackStruct(buf.bytes()), }; - // wpi::println("->[client] Got pong: {} {} {} {}", pong.version, + // wpi::util::println("->[client] Got pong: {} {} {} {}", pong.version, // pong.message_id, pong.client_time, pong.server_time); if (pong.version != 1) { - wpi::println("Bad version from server?"); + wpi::util::println("Bad version from server?"); return; } if (pong.message_id != 2) { - wpi::println("Bad message id from server?"); + wpi::util::println("Bad message id from server?"); return; } @@ -143,9 +144,9 @@ void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes, UpdateStatistics(pong_local_time, ping, pong); // using std::cout; - // wpi::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2, + // wpi::util::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2, // serverTimeOffsetUs); - // wpi::println("Estimated server time {} s", + // wpi::util::println("Estimated server time {} s", // (m_timeProvider() + serverTimeOffsetUs) / 1000000.0); } @@ -153,37 +154,37 @@ wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server, int remote_port, std::chrono::milliseconds ping_delay) : m_logger(::ClientLoggerFunc), - m_timeProvider(nt::Now), + m_timeProvider(wpi::nt::Now), m_udp{}, m_pingTimer{}, m_serverIP{server}, m_serverPort{remote_port}, m_loopDelay(ping_delay) { - // wpi::println("Starting client (with server address {}:{})", server, + // wpi::util::println("Starting client (with server address {}:{})", server, // remote_port); } void wpi::tsp::TimeSyncClient::Start() { - // wpi::println("Connecting received"); + // wpi::util::println("Connecting received"); - m_loopRunner.ExecSync([this](uv::Loop&) { + m_loopRunner.ExecSync([this](wpi::net::uv::Loop&) { struct sockaddr_in serverAddr; - uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr); + wpi::net::uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr); - m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)}; - m_pingTimer = {wpi::uv::Timer::Create(m_loopRunner.GetLoop())}; + m_udp = {wpi::net::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)}; + m_pingTimer = {wpi::net::uv::Timer::Create(m_loopRunner.GetLoop())}; m_udp->Connect(serverAddr); m_udp->received.connect(&wpi::tsp::TimeSyncClient::UdpCallback, this); m_udp->StartRecv(); }); - // wpi::println("Starting pinger"); + // wpi::util::println("Starting pinger"); using namespace std::chrono_literals; m_pingTimer->timeout.connect(&wpi::tsp::TimeSyncClient::Tick, this); m_loopRunner.ExecSync( - [this](uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); }); + [this](wpi::net::uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); }); } void wpi::tsp::TimeSyncClient::Stop() { m_loopRunner.Stop(); } diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp index 4abd59ae3c..70e4f7974e 100644 --- a/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp +++ b/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp @@ -21,14 +21,13 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include "net/TimeSyncStructs.h" +#include static void ServerLoggerFunc(unsigned int level, const char* file, unsigned int line, const char* msg) { @@ -51,12 +50,12 @@ static void ServerLoggerFunc(unsigned int level, const char* file, line); } -void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n, +void wpi::tsp::TimeSyncServer::UdpCallback(wpi::net::uv::Buffer& data, size_t n, const sockaddr& sender, unsigned flags) { - // wpi::println("TimeSyncServer got ping!"); + // wpi::util::println("TimeSyncServer got ping!"); - TspPing ping{wpi::UnpackStruct(data.bytes())}; + TspPing ping{wpi::util::UnpackStruct(data.bytes())}; if (ping.version != 1) { WPI_ERROR(m_logger, "Bad version from client?"); @@ -72,16 +71,16 @@ void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n, TspPong pong{ping, current_time}; pong.message_id = 2; - wpi::SmallVector::GetSize()> pongData( - wpi::Struct::GetSize()); - wpi::PackStruct(pongData, pong); + wpi::util::SmallVector::GetSize()> + pongData(wpi::util::Struct::GetSize()); + wpi::util::PackStruct(pongData, pong); // Wrap our buffer - pongData should free itself for free - wpi::uv::Buffer pongBuf{pongData}; - int sent = - m_udp->TrySend(sender, wpi::SmallVector{pongBuf}); - // wpi::println("Pong ret: {}", sent); - if (static_cast(sent) != wpi::Struct::GetSize()) { + wpi::net::uv::Buffer pongBuf{pongData}; + int sent = m_udp->TrySend( + sender, wpi::util::SmallVector{pongBuf}); + // wpi::util::println("Pong ret: {}", sent); + if (static_cast(sent) != wpi::util::Struct::GetSize()) { WPI_ERROR(m_logger, "Didn't send the whole pong back?"); return; } @@ -94,13 +93,13 @@ void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n, wpi::tsp::TimeSyncServer::TimeSyncServer(int port) : m_logger{::ServerLoggerFunc}, - m_timeProvider{nt::Now}, + m_timeProvider{wpi::nt::Now}, m_udp{}, m_port(port) {} void wpi::tsp::TimeSyncServer::Start() { - m_loopRunner.ExecSync([this](uv::Loop&) { - m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)}; + m_loopRunner.ExecSync([this](wpi::net::uv::Loop&) { + m_udp = {wpi::net::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)}; m_udp->Bind("0.0.0.0", m_port); m_udp->received.connect(&wpi::tsp::TimeSyncServer::UdpCallback, this); m_udp->StartRecv(); diff --git a/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp b/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp index a2bc3446ee..aefa359be4 100644 --- a/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp +++ b/photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/wrap/casadi_wrapper.cpp @@ -24,8 +24,8 @@ #include #include #include -#include -#include +#include +#include #include "../generate/constrained_solvepnp_10_tags_fixed.h" #include "../generate/constrained_solvepnp_10_tags_free.h" @@ -153,7 +153,7 @@ struct ProblemState { #undef MAKE_ARGV }; -wpi::expected +wpi::util::expected constrained_solvepnp::do_optimization( bool heading_free, int nTags, constrained_solvepnp::CameraCalibration cameraCal, @@ -168,7 +168,7 @@ constrained_solvepnp::do_optimization( point_observations.cols() != (nTags * 4)) { if constexpr (VERBOSE) fmt::println("Got unexpected num cols!"); // TODO find a new error code - return wpi::unexpected{ + return wpi::util::unexpected{ slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS}; } @@ -198,7 +198,7 @@ constrained_solvepnp::do_optimization( auto problemOpt = createProblem(nTags, heading_free); if (!problemOpt) { - return wpi::unexpected{ + return wpi::util::unexpected{ slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS}; } @@ -225,11 +225,11 @@ constrained_solvepnp::do_optimization( constexpr double ERROR_TOL = 1e-4; for (int iter = 0; iter < 100; iter++) { - auto iter_start = wpi::Now(); + auto iter_start = wpi::util::Now(); // Check for diverging iterates if (x.template lpNorm() > 1e20 || !x.allFinite()) { - return wpi::unexpected{slp::ExitStatus::DIVERGING_ITERATES}; + return wpi::util::unexpected{slp::ExitStatus::DIVERGING_ITERATES}; } GradMat g = pState.calculateGradJ(x); @@ -250,7 +250,7 @@ constrained_solvepnp::do_optimization( auto H_ldlt = H.ldlt(); if (H_ldlt.info() != Eigen::Success) { fmt::println(stderr, "LDLT decomp failed! H=\n{}", H); - return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; + return wpi::util::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; } // Make sure H is positive definite (all eigenvalues are > 0) @@ -274,7 +274,7 @@ constrained_solvepnp::do_optimization( if (H_ldlt.info() != Eigen::Success) { fmt::println(stderr, "LDLT decomp failed! H=\n{}", H); - return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; + return wpi::util::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; } // If our eigenvalues aren't positive definite, pick a new δ for next @@ -284,7 +284,7 @@ constrained_solvepnp::do_optimization( // If the Hessian perturbation is too high, report failure if (δ > 1e20) { - return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; + return wpi::util::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; } } else { // Done! @@ -295,7 +295,7 @@ constrained_solvepnp::do_optimization( } if (i_reg == MAX_REG_STEPS) { - return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; + return wpi::util::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; } } else { // std::printf("Already regularized\n"); @@ -338,12 +338,12 @@ constrained_solvepnp::do_optimization( // If our step size shrank too much, report local infesibility if (alpha < α_min_frac * γConstraint) { - return wpi::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; + return wpi::util::unexpected{slp::ExitStatus::LOCALLY_INFEASIBLE}; } } } - auto iter_end = wpi::Now(); + auto iter_end = wpi::util::Now(); if constexpr (VERBOSE) { fmt::println( "{}: {} uS, ‖∇J‖={}, α={} ({} refinement steps), {} regularization " diff --git a/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp index 4731e8f7c8..b65f2f388b 100644 --- a/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp +++ b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp @@ -25,15 +25,15 @@ namespace photon { namespace VisionEstimation { -std::vector GetVisibleLayoutTags( +std::vector GetVisibleLayoutTags( const std::vector& visTags, - const frc::AprilTagFieldLayout& layout) { - std::vector retVal{}; + const wpi::apriltag::AprilTagFieldLayout& layout) { + std::vector retVal{}; for (const auto& tag : visTags) { int id = tag.GetFiducialId(); auto maybePose = layout.GetTagPose(id); if (maybePose) { - retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + retVal.emplace_back(wpi::apriltag::AprilTag{id, maybePose.value()}); } } return retVal; @@ -43,19 +43,20 @@ std::optional EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, - const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + const wpi::apriltag::AprilTagFieldLayout& layout, + const TargetModel& tagModel) { if (visTags.size() == 0) { return PnpResult(); } std::vector corners{}; - std::vector knownTags{}; + std::vector knownTags{}; for (const auto& tgt : visTags) { int id = tgt.GetFiducialId(); auto maybePose = layout.GetTagPose(id); if (maybePose) { - knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + knownTags.emplace_back(wpi::apriltag::AprilTag{id, maybePose.value()}); auto currentCorners = tgt.GetDetectedCorners(); corners.insert(corners.end(), currentCorners.begin(), currentCorners.end()); @@ -73,22 +74,22 @@ std::optional EstimateCamPosePNP( if (!camToTag) { return PnpResult{}; } - frc::Pose3d bestPose = + wpi::math::Pose3d bestPose = knownTags[0].pose.TransformBy(camToTag->best.Inverse()); - frc::Pose3d altPose{}; + wpi::math::Pose3d altPose{}; if (camToTag->ambiguity != 0) { altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse()); } - frc::Pose3d o{}; + wpi::math::Pose3d o{}; PnpResult result{}; - result.best = frc::Transform3d{o, bestPose}; - result.alt = frc::Transform3d{o, altPose}; + result.best = wpi::math::Transform3d{o, bestPose}; + result.alt = wpi::math::Transform3d{o, altPose}; result.ambiguity = camToTag->ambiguity; result.bestReprojErr = camToTag->bestReprojErr; result.altReprojErr = camToTag->altReprojErr; return result; } else { - std::vector objectTrls{}; + std::vector objectTrls{}; for (const auto& tag : knownTags) { auto verts = tagModel.GetFieldVertices(tag.pose); objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); @@ -109,21 +110,23 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, - const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed, - const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel, - bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac) { + const wpi::math::Transform3d& robot2Camera, + const wpi::math::Pose3d& robotPoseSeed, + const wpi::apriltag::AprilTagFieldLayout& layout, + const photon::TargetModel& tagModel, bool headingFree, + wpi::math::Rotation2d gyroTheta, double gyroErrorScaleFac) { if (visTags.size() == 0) { return photon::PnpResult(); } std::vector corners{}; - std::vector knownTags{}; + std::vector knownTags{}; for (const auto& tgt : visTags) { int id = tgt.GetFiducialId(); auto maybePose = layout.GetTagPose(id); if (maybePose) { - knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + knownTags.emplace_back(wpi::apriltag::AprilTag{id, maybePose.value()}); auto currentCorners = tgt.GetDetectedCorners(); corners.insert(corners.end(), currentCorners.begin(), currentCorners.end()); @@ -158,7 +161,7 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( pointObservations(1, i) = points[i].y; } - std::vector objectTrls{}; + std::vector objectTrls{}; for (const auto& tag : knownTags) { auto verts = tagModel.GetFieldVertices(tag.pose); objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); @@ -175,7 +178,7 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( field2points(3, i) = 1; } - frc::Pose2d guess2 = robotPoseSeed.ToPose2d(); + wpi::math::Pose2d guess2 = robotPoseSeed.ToPose2d(); constrained_solvepnp::CameraCalibration cameraCal{ cameraMatrix(0, 0), @@ -188,7 +191,7 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( guess2.X().value(), guess2.Y().value(), guess2.Rotation().Radians().value()}; - wpi::expected result = + wpi::util::expected result = constrained_solvepnp::do_optimization( headingFree, knownTags.size(), cameraCal, robotToCamera, guessMat, field2points, pointObservations, gyroTheta.Radians().value(), @@ -199,9 +202,9 @@ std::optional EstimateRobotPoseConstrainedSolvePNP( } else { photon::PnpResult res{}; - res.best = frc::Transform3d{frc::Transform2d{ - units::meter_t{result.value()[0]}, units::meter_t{result.value()[1]}, - frc::Rotation2d{units::radian_t{result.value()[2]}}}}; + res.best = wpi::math::Transform3d{wpi::math::Transform2d{ + wpi::units::meter_t{result.value()[0]}, wpi::units::meter_t{result.value()[1]}, + wpi::math::Rotation2d{wpi::units::radian_t{result.value()[2]}}}}; return res; } diff --git a/photon-targeting/src/main/native/include/net/TimeSyncClient.h b/photon-targeting/src/main/native/include/net/TimeSyncClient.h index 7c3194a68f..69508b5db3 100644 --- a/photon-targeting/src/main/native/include/net/TimeSyncClient.h +++ b/photon-targeting/src/main/native/include/net/TimeSyncClient.h @@ -24,12 +24,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "TimeSyncStructs.h" @@ -47,12 +47,12 @@ class TimeSyncClient { }; private: - using SharedUdpPtr = std::shared_ptr; - using SharedTimerPtr = std::shared_ptr; + using SharedUdpPtr = std::shared_ptr; + using SharedTimerPtr = std::shared_ptr; - EventLoopRunner m_loopRunner{}; + wpi::net::EventLoopRunner m_loopRunner{}; - wpi::Logger m_logger; + wpi::util::Logger m_logger; std::function m_timeProvider; SharedUdpPtr m_udp; @@ -70,12 +70,12 @@ class TimeSyncClient { TspPing m_lastPing{}; // 30s is a reasonable guess - frc::MedianFilter m_lastOffsets{30}; + wpi::math::MedianFilter m_lastOffsets{30}; void Tick(); - void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender, - unsigned flags); + void UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, + const sockaddr& sender, unsigned flags); public: TimeSyncClient(std::string_view server, int remote_port, diff --git a/photon-targeting/src/main/native/include/net/TimeSyncServer.h b/photon-targeting/src/main/native/include/net/TimeSyncServer.h index 21f2a04a89..2bb8ccc35f 100644 --- a/photon-targeting/src/main/native/include/net/TimeSyncServer.h +++ b/photon-targeting/src/main/native/include/net/TimeSyncServer.h @@ -24,20 +24,20 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace wpi { namespace tsp { class TimeSyncServer { - using SharedUdpPtr = std::shared_ptr; + using SharedUdpPtr = std::shared_ptr; - EventLoopRunner m_loopRunner{}; + wpi::net::EventLoopRunner m_loopRunner{}; - wpi::Logger m_logger; + wpi::util::Logger m_logger; std::function m_timeProvider; SharedUdpPtr m_udp; int m_port; @@ -45,7 +45,7 @@ class TimeSyncServer { std::thread m_listener; private: - void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender, + void UdpCallback(wpi::net::uv::Buffer& buf, size_t nbytes, const sockaddr& sender, unsigned flags); public: diff --git a/photon-targeting/src/main/native/include/net/TimeSyncStructs.h b/photon-targeting/src/main/native/include/net/TimeSyncStructs.h index 887025972b..c24b43545d 100644 --- a/photon-targeting/src/main/native/include/net/TimeSyncStructs.h +++ b/photon-targeting/src/main/native/include/net/TimeSyncStructs.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace wpi { namespace tsp { @@ -39,7 +39,7 @@ struct TspPong : public TspPing { } // namespace wpi template <> -struct wpi::Struct { +struct wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "TspPing"; } static constexpr size_t GetSize() { return 10; } static constexpr std::string_view GetSchema() { @@ -48,20 +48,20 @@ struct wpi::Struct { static wpi::tsp::TspPing Unpack(std::span data) { return wpi::tsp::TspPing{ - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), + wpi::util::UnpackStruct(data), + wpi::util::UnpackStruct(data), + wpi::util::UnpackStruct(data), }; } static void Pack(std::span data, const wpi::tsp::TspPing& value) { - wpi::PackStruct<0>(data, value.version); - wpi::PackStruct<1>(data, value.message_id); - wpi::PackStruct<2>(data, value.client_time); + wpi::util::PackStruct<0>(data, value.version); + wpi::util::PackStruct<1>(data, value.message_id); + wpi::util::PackStruct<2>(data, value.client_time); } }; template <> -struct wpi::Struct { +struct wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "TspPong"; } static constexpr size_t GetSize() { return 18; } static constexpr std::string_view GetSchema() { @@ -72,20 +72,20 @@ struct wpi::Struct { static wpi::tsp::TspPong Unpack(std::span data) { return wpi::tsp::TspPong{ wpi::tsp::TspPing{ - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), + wpi::util::UnpackStruct(data), + wpi::util::UnpackStruct(data), + wpi::util::UnpackStruct(data), }, - wpi::UnpackStruct(data), + wpi::util::UnpackStruct(data), }; } static void Pack(std::span data, const wpi::tsp::TspPong& value) { - wpi::PackStruct<0>(data, value.version); - wpi::PackStruct<1>(data, value.message_id); - wpi::PackStruct<2>(data, value.client_time); - wpi::PackStruct<10>(data, value.server_time); + wpi::util::PackStruct<0>(data, value.version); + wpi::util::PackStruct<1>(data, value.message_id); + wpi::util::PackStruct<2>(data, value.client_time); + wpi::util::PackStruct<10>(data, value.server_time); } }; -static_assert(wpi::StructSerializable); -static_assert(wpi::StructSerializable); +static_assert(wpi::util::StructSerializable); +static_assert(wpi::util::StructSerializable); diff --git a/photon-targeting/src/main/native/include/photon/constrained_solvepnp/wrap/casadi_wrapper.h b/photon-targeting/src/main/native/include/photon/constrained_solvepnp/wrap/casadi_wrapper.h index c71629f730..921e383096 100644 --- a/photon-targeting/src/main/native/include/photon/constrained_solvepnp/wrap/casadi_wrapper.h +++ b/photon-targeting/src/main/native/include/photon/constrained_solvepnp/wrap/casadi_wrapper.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace constrained_solvepnp { using casadi_real = double; @@ -40,7 +40,7 @@ using RobotStateMat = Eigen::Matrix; * to this. The number of columns in field2points and point_observations just be * exactly 4x nTags. */ -wpi::expected do_optimization( +wpi::util::expected do_optimization( bool heading_free, int nTags, CameraCalibration cameraCal, // Note that casadi is column major, apparently Eigen::Matrix robot2camera, diff --git a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h index 0563ebd385..840ee8300c 100644 --- a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h +++ b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h @@ -24,7 +24,7 @@ #include #include -#include +#include namespace photon { @@ -90,14 +90,14 @@ class Packet { inline size_t GetDataSize() const { return packetData.size(); } template - requires wpi::StructSerializable + requires wpi::util::StructSerializable inline void Pack(const T& value) { // as WPI struct stuff assumes constant data length - reserve at least // enough new space for our new member - size_t newWritePos = writePos + wpi::GetStructSize(); + size_t newWritePos = writePos + wpi::util::GetStructSize(); packetData.resize(newWritePos); - wpi::PackStruct( + wpi::util::PackStruct( std::span{packetData.begin() + writePos, packetData.end()}, value); @@ -111,12 +111,12 @@ class Packet { } template - requires wpi::StructSerializable + requires wpi::util::StructSerializable inline T Unpack() { // Unpack this member, starting at readPos - T ret = wpi::UnpackStruct( + T ret = wpi::util::UnpackStruct( std::span{packetData.begin() + readPos, packetData.end()}); - readPos += wpi::GetStructSize(); + readPos += wpi::util::GetStructSize(); return ret; } diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h index a0a0fb0e61..d073bba99b 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -17,45 +17,45 @@ #pragma once -#include +#include namespace photon { class CameraTargetRelation { public: - const frc::Pose3d camPose; - const frc::Transform3d camToTarg; - const units::meter_t camToTargDist; - const units::meter_t camToTargDistXY; - const frc::Rotation2d camToTargYaw; - const frc::Rotation2d camToTargPitch; + const wpi::math::Pose3d camPose; + const wpi::math::Transform3d camToTarg; + const wpi::units::meter_t camToTargDist; + const wpi::units::meter_t camToTargDistXY; + const wpi::math::Rotation2d camToTargYaw; + const wpi::math::Rotation2d camToTargPitch; - const frc::Rotation2d camToTargAngle; + const wpi::math::Rotation2d camToTargAngle; - const frc::Transform3d targToCam; - const frc::Rotation2d targToCamYaw; - const frc::Rotation2d targToCamPitch; + const wpi::math::Transform3d targToCam; + const wpi::math::Rotation2d targToCamYaw; + const wpi::math::Rotation2d targToCamPitch; - const frc::Rotation2d targToCamAngle; + const wpi::math::Rotation2d targToCamAngle; - CameraTargetRelation(const frc::Pose3d& cameraPose, - const frc::Pose3d& targetPose) + CameraTargetRelation(const wpi::math::Pose3d& cameraPose, + const wpi::math::Pose3d& targetPose) : camPose(cameraPose), - camToTarg(frc::Transform3d{cameraPose, targetPose}), + camToTarg(wpi::math::Transform3d{cameraPose, targetPose}), camToTargDist(camToTarg.Translation().Norm()), - camToTargDistXY(units::math::hypot(camToTarg.Translation().X(), + camToTargDistXY(wpi::units::math::hypot(camToTarg.Translation().X(), camToTarg.Translation().Y())), - camToTargYaw(frc::Rotation2d{camToTarg.X().to(), - camToTarg.Y().to()}), - camToTargPitch(frc::Rotation2d{camToTargDistXY.to(), - -camToTarg.Z().to()}), - camToTargAngle(frc::Rotation2d{units::math::hypot( + camToTargYaw(wpi::math::Rotation2d{camToTarg.X().to(), + camToTarg.Y().to()}), + camToTargPitch(wpi::math::Rotation2d{camToTargDistXY.to(), + -camToTarg.Z().to()}), + camToTargAngle(wpi::math::Rotation2d{wpi::units::math::hypot( camToTargYaw.Radians(), camToTargPitch.Radians())}), - targToCam(frc::Transform3d{targetPose, cameraPose}), - targToCamYaw(frc::Rotation2d{targToCam.X().to(), - targToCam.Y().to()}), - targToCamPitch(frc::Rotation2d{camToTargDistXY.to(), - -targToCam.Z().to()}), - targToCamAngle(frc::Rotation2d{units::math::hypot( + targToCam(wpi::math::Transform3d{targetPose, cameraPose}), + targToCamYaw(wpi::math::Rotation2d{targToCam.X().to(), + targToCam.Y().to()}), + targToCamPitch(wpi::math::Rotation2d{camToTargDistXY.to(), + -targToCam.Z().to()}), + targToCamAngle(wpi::math::Rotation2d{wpi::units::math::hypot( targToCamYaw.Radians(), targToCamPitch.Radians())}) {} }; } // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index 96fd00dce6..f63b7482a4 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -36,9 +36,9 @@ namespace photon { namespace OpenCVHelp { -static frc::Rotation3d NWU_TO_EDN{ +static wpi::math::Rotation3d NWU_TO_EDN{ (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()}; -static frc::Rotation3d EDN_TO_NWU{ +static wpi::math::Rotation3d EDN_TO_NWU{ (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; [[maybe_unused]] static std::vector GetConvexHull( @@ -57,20 +57,22 @@ static frc::Rotation3d EDN_TO_NWU{ return cv::minAreaRect(points); } -static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) { +static wpi::math::Translation3d TranslationNWUtoEDN( + const wpi::math::Translation3d& trl) { return trl.RotateBy(NWU_TO_EDN); } -static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) { +static wpi::math::Rotation3d RotationNWUtoEDN( + const wpi::math::Rotation3d& rot) { return -NWU_TO_EDN + (rot + NWU_TO_EDN); } static std::vector TranslationToTVec( - const std::vector& translations) { + const std::vector& translations) { std::vector retVal; retVal.reserve(translations.size()); for (size_t i = 0; i < translations.size(); i++) { - frc::Translation3d trl = TranslationNWUtoEDN(translations[i]); + wpi::math::Translation3d trl = TranslationNWUtoEDN(translations[i]); retVal.emplace_back(cv::Point3f{trl.X().to(), trl.Y().to(), trl.Z().to()}); } @@ -78,9 +80,9 @@ static std::vector TranslationToTVec( } static std::vector RotationToRVec( - const frc::Rotation3d& rotation) { + const wpi::math::Rotation3d& rotation) { std::vector retVal{}; - frc::Rotation3d rot = RotationNWUtoEDN(rotation); + wpi::math::Rotation3d rot = RotationNWUtoEDN(rotation); retVal.emplace_back(cv::Point3d{ rot.GetQuaternion().ToRotationVector()(0), rot.GetQuaternion().ToRotationVector()(1), @@ -147,7 +149,7 @@ static std::vector RotationToRVec( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const RotTrlTransform3d& camRt, - const std::vector& objectTranslations) { + const std::vector& objectTranslations) { std::vector objectPoints = TranslationToTVec(objectTranslations); std::vector rvec = RotationToRVec(camRt.GetRotation()); std::vector tvec = TranslationToTVec({camRt.GetTranslation()}); @@ -177,37 +179,39 @@ static std::vector ReorderCircular(const std::vector elements, return reordered; } -static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) { +static wpi::math::Translation3d TranslationEDNToNWU( + const wpi::math::Translation3d& trl) { return trl.RotateBy(EDN_TO_NWU); } -static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) { +static wpi::math::Rotation3d RotationEDNToNWU( + const wpi::math::Rotation3d& rot) { return -EDN_TO_NWU + (rot + EDN_TO_NWU); } -static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { +static wpi::math::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { cv::Vec3f data{}; cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; tvecInput.convertTo(wrapped, CV_32F); data = wrapped.at(cv::Point{0, 0}); - return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]}, - units::meter_t{data[1]}, - units::meter_t{data[2]}}); + return TranslationEDNToNWU(wpi::math::Translation3d{wpi::units::meter_t{data[0]}, + wpi::units::meter_t{data[1]}, + wpi::units::meter_t{data[2]}}); } -static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { +static wpi::math::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { cv::Vec3f data{}; cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; rvecInput.convertTo(wrapped, CV_32F); data = wrapped.at(cv::Point{0, 0}); return RotationEDNToNWU( - frc::Rotation3d{Eigen::Vector3d{data[0], data[1], data[2]}}); + wpi::math::Rotation3d{Eigen::Vector3d{data[0], data[1], data[2]}}); } [[maybe_unused]] static std::optional SolvePNP_Square( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, - std::vector modelTrls, + std::vector modelTrls, std::vector imagePoints) { modelTrls = ReorderCircular(modelTrls, true, -1); imagePoints = ReorderCircular(imagePoints, true, -1); @@ -224,8 +228,8 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { cv::eigen2cv(distCoeffs, distCoeffsMat); cv::Vec2d errors{}; - frc::Transform3d best{}; - std::optional alt{std::nullopt}; + wpi::math::Transform3d best{}; + std::optional alt{std::nullopt}; for (int tries = 0; tries < 2; tries++) { cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, @@ -233,12 +237,12 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { reprojectionError); errors = reprojectionError.at(cv::Point{0, 0}); - best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), - RVecToRotation(rvecs[0])}; + best = wpi::math::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; if (tvecs.size() > 1) { - alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)), - RVecToRotation(rvecs[1])}; + alt = wpi::math::Transform3d{TVecToTranslation(tvecs.at(1)), + RVecToRotation(rvecs[1])}; } if (!std::isnan(errors[0])) { @@ -274,7 +278,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { [[maybe_unused]] static std::optional SolvePNP_SQPNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, - std::vector modelTrls, + std::vector modelTrls, std::vector imagePoints) { std::vector objectMat = TranslationToTVec(modelTrls); std::vector rvecs{}; @@ -289,15 +293,15 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { cv::eigen2cv(distCoeffs, distCoeffsMat); float error = 0; - frc::Transform3d best{}; + wpi::math::Transform3d best{}; cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec, reprojectionError); error = reprojectionError.at(cv::Point{0, 0}); - best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), - RVecToRotation(rvecs[0])}; + best = wpi::math::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; if (std::isnan(error)) { fmt::print("SolvePNP_Square failed!\n"); diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h index 5c08f937f8..0f5d733fc6 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -19,49 +19,54 @@ #include -#include -#include -#include +#include +#include +#include namespace photon { class RotTrlTransform3d { public: - RotTrlTransform3d(const frc::Rotation3d& newRot, - const frc::Translation3d& newTrl) + RotTrlTransform3d(const wpi::math::Rotation3d& newRot, + const wpi::math::Translation3d& newTrl) : trl{newTrl}, rot{newRot} {} - RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) + RotTrlTransform3d(const wpi::math::Pose3d& initial, + const wpi::math::Pose3d& last) : trl{last.Translation() - initial.Translation().RotateBy( last.Rotation() - initial.Rotation())}, rot{last.Rotation() - initial.Rotation()} {} - explicit RotTrlTransform3d(const frc::Transform3d& trf) + explicit RotTrlTransform3d(const wpi::math::Transform3d& trf) : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} RotTrlTransform3d() - : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {} + : RotTrlTransform3d(wpi::math::Rotation3d{}, wpi::math::Translation3d{}) { + } - static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) { + static RotTrlTransform3d MakeRelativeTo(const wpi::math::Pose3d& pose) { return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse(); } RotTrlTransform3d Inverse() const { - frc::Rotation3d invRot = -rot; - frc::Translation3d invTrl = -(trl.RotateBy(invRot)); + wpi::math::Rotation3d invRot = -rot; + wpi::math::Translation3d invTrl = -(trl.RotateBy(invRot)); return RotTrlTransform3d{invRot, invTrl}; } - frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; } + wpi::math::Transform3d GetTransform() const { + return wpi::math::Transform3d{trl, rot}; + } - frc::Translation3d GetTranslation() const { return trl; } + wpi::math::Translation3d GetTranslation() const { return trl; } - frc::Rotation3d GetRotation() const { return rot; } + wpi::math::Rotation3d GetRotation() const { return rot; } - frc::Translation3d Apply(const frc::Translation3d& trlToApply) const { + wpi::math::Translation3d Apply( + const wpi::math::Translation3d& trlToApply) const { return trlToApply.RotateBy(rot) + trl; } - std::vector ApplyTrls( - const std::vector& trls) const { - std::vector retVal; + std::vector ApplyTrls( + const std::vector& trls) const { + std::vector retVal; retVal.reserve(trls.size()); for (const auto& currentTrl : trls) { retVal.emplace_back(Apply(currentTrl)); @@ -69,13 +74,13 @@ class RotTrlTransform3d { return retVal; } - frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const { + wpi::math::Rotation3d Apply(const wpi::math::Rotation3d& rotToApply) const { return rotToApply + rot; } - std::vector ApplyTrls( - const std::vector& rots) const { - std::vector retVal; + std::vector ApplyTrls( + const std::vector& rots) const { + std::vector retVal; retVal.reserve(rots.size()); for (const auto& currentRot : rots) { retVal.emplace_back(Apply(currentRot)); @@ -83,14 +88,14 @@ class RotTrlTransform3d { return retVal; } - frc::Pose3d Apply(const frc::Pose3d& poseToApply) const { - return frc::Pose3d{Apply(poseToApply.Translation()), - Apply(poseToApply.Rotation())}; + wpi::math::Pose3d Apply(const wpi::math::Pose3d& poseToApply) const { + return wpi::math::Pose3d{Apply(poseToApply.Translation()), + Apply(poseToApply.Rotation())}; } - std::vector ApplyPoses( - const std::vector& poses) const { - std::vector retVal; + std::vector ApplyPoses( + const std::vector& poses) const { + std::vector retVal; retVal.reserve(poses.size()); for (const auto& currentPose : poses) { retVal.emplace_back(Apply(currentPose)); @@ -99,7 +104,7 @@ class RotTrlTransform3d { } private: - const frc::Translation3d trl; - const frc::Rotation3d rot; + const wpi::math::Translation3d trl; + const wpi::math::Rotation3d rot; }; } // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h index 64392fdb11..be2b6ab6bd 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h +++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h @@ -19,49 +19,50 @@ #include -#include -#include +#include +#include #include "RotTrlTransform3d.h" namespace photon { class TargetModel { public: - TargetModel(units::meter_t width, units::meter_t height) - : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0}, - frc::Translation3d{0_m, width / 2.0, -height / 2.0}, - frc::Translation3d{0_m, width / 2.0, height / 2.0}, - frc::Translation3d{0_m, -width / 2.0, height / 2.0}}), + TargetModel(wpi::units::meter_t width, wpi::units::meter_t height) + : vertices({wpi::math::Translation3d{0_m, -width / 2.0, -height / 2.0}, + wpi::math::Translation3d{0_m, width / 2.0, -height / 2.0}, + wpi::math::Translation3d{0_m, width / 2.0, height / 2.0}, + wpi::math::Translation3d{0_m, -width / 2.0, height / 2.0}}), isPlanar(true), isSpherical(false) {} - TargetModel(units::meter_t length, units::meter_t width, - units::meter_t height) + TargetModel(wpi::units::meter_t length, wpi::units::meter_t width, + wpi::units::meter_t height) : TargetModel({ - frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, - frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, - frc::Translation3d{length / 2.0, width / 2.0, height / 2.0}, - frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, - frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, - frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, - frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, - frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0}, + wpi::math::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, + wpi::math::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, + wpi::math::Translation3d{length / 2.0, width / 2.0, height / 2.0}, + wpi::math::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, + wpi::math::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, + wpi::math::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, + wpi::math::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, + wpi::math::Translation3d{-length / 2.0, -width / 2.0, + -height / 2.0}, }) {} - explicit TargetModel(units::meter_t diameter) + explicit TargetModel(wpi::units::meter_t diameter) : vertices({ - frc::Translation3d{0_m, -diameter / 2.0, 0_m}, - frc::Translation3d{0_m, 0_m, -diameter / 2.0}, - frc::Translation3d{0_m, diameter / 2.0, 0_m}, - frc::Translation3d{0_m, 0_m, diameter / 2.0}, + wpi::math::Translation3d{0_m, -diameter / 2.0, 0_m}, + wpi::math::Translation3d{0_m, 0_m, -diameter / 2.0}, + wpi::math::Translation3d{0_m, diameter / 2.0, 0_m}, + wpi::math::Translation3d{0_m, 0_m, diameter / 2.0}, }), isPlanar(false), isSpherical(true) {} - explicit TargetModel(const std::vector& verts) + explicit TargetModel(const std::vector& verts) : isSpherical(false) { if (verts.size() <= 2) { - vertices = std::vector(); + vertices = std::vector(); isPlanar = false; } else { bool cornersPlanar = true; @@ -75,11 +76,11 @@ class TargetModel { vertices = verts; } - std::vector GetFieldVertices( - const frc::Pose3d& targetPose) const { + std::vector GetFieldVertices( + const wpi::math::Pose3d& targetPose) const { RotTrlTransform3d basisChange{targetPose.Rotation(), targetPose.Translation()}; - std::vector retVal; + std::vector retVal; retVal.reserve(vertices.size()); for (const auto& vert : vertices) { retVal.emplace_back(basisChange.Apply(vert)); @@ -87,25 +88,27 @@ class TargetModel { return retVal; } - static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl, - const frc::Translation3d& cameraTrl) { - frc::Translation3d relCam = cameraTrl - tgtTrl; - frc::Rotation3d orientToCam = frc::Rotation3d{ + static wpi::math::Pose3d GetOrientedPose( + const wpi::math::Translation3d& tgtTrl, + const wpi::math::Translation3d& cameraTrl) { + wpi::math::Translation3d relCam = cameraTrl - tgtTrl; + wpi::math::Rotation3d orientToCam = wpi::math::Rotation3d{ 0_rad, - frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to(), - -relCam.Z().to()} + wpi::math::Rotation2d{ + wpi::units::math::hypot(relCam.X(), relCam.Y()).to(), + -relCam.Z().to()} .Radians(), - frc::Rotation2d{relCam.X().to(), relCam.Y().to()} + wpi::math::Rotation2d{relCam.X().to(), relCam.Y().to()} .Radians()}; - return frc::Pose3d{tgtTrl, orientToCam}; + return wpi::math::Pose3d{tgtTrl, orientToCam}; } - std::vector GetVertices() const { return vertices; } + std::vector GetVertices() const { return vertices; } bool GetIsPlanar() const { return isPlanar; } bool GetIsSpherical() const { return isSpherical; } private: - std::vector vertices; + std::vector vertices; bool isPlanar; bool isSpherical; }; diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h index e9445ed8fb..56e532a18f 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include "TargetModel.h" #include "photon/targeting/PhotonTrackedTarget.h" @@ -30,23 +30,26 @@ namespace photon { namespace VisionEstimation { -std::vector GetVisibleLayoutTags( +std::vector GetVisibleLayoutTags( const std::vector& visTags, - const frc::AprilTagFieldLayout& layout); + const wpi::apriltag::AprilTagFieldLayout& layout); std::optional EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, - const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel); + const wpi::apriltag::AprilTagFieldLayout& layout, + const TargetModel& tagModel); std::optional EstimateRobotPoseConstrainedSolvePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, - const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed, - const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel, - bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac); + const wpi::math::Transform3d& robot2Camera, + const wpi::math::Pose3d& robotPoseSeed, + const wpi::apriltag::AprilTagFieldLayout& layout, + const photon::TargetModel& tagModel, bool headingFree, + wpi::math::Rotation2d gyroTheta, double gyroErrorScaleFac); } // namespace VisionEstimation } // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h index 1e905d9a55..c5a8957fdd 100644 --- a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h +++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h @@ -20,14 +20,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include namespace photon { const std::string PhotonPipelineResult_TYPE_STRING = @@ -36,35 +36,35 @@ const std::string PhotonPipelineResult_TYPE_STRING = class NTTopicSet { public: - std::shared_ptr subTable; - nt::RawPublisher rawBytesEntry; + std::shared_ptr subTable; + wpi::nt::RawPublisher rawBytesEntry; - nt::IntegerPublisher pipelineIndexPublisher; - nt::IntegerSubscriber pipelineIndexRequestSub; + wpi::nt::IntegerPublisher pipelineIndexPublisher; + wpi::nt::IntegerSubscriber pipelineIndexRequestSub; - nt::BooleanTopic driverModeEntry; - nt::BooleanPublisher driverModePublisher; - nt::BooleanSubscriber driverModeSubscriber; + wpi::nt::BooleanTopic driverModeEntry; + wpi::nt::BooleanPublisher driverModePublisher; + wpi::nt::BooleanSubscriber driverModeSubscriber; - nt::DoublePublisher latencyMillisEntry; - nt::BooleanPublisher hasTargetEntry; - nt::DoublePublisher targetPitchEntry; - nt::DoublePublisher targetYawEntry; - nt::DoublePublisher targetAreaEntry; - nt::StructPublisher targetPoseEntry; - nt::DoublePublisher targetSkewEntry; + wpi::nt::DoublePublisher latencyMillisEntry; + wpi::nt::BooleanPublisher hasTargetEntry; + wpi::nt::DoublePublisher targetPitchEntry; + wpi::nt::DoublePublisher targetYawEntry; + wpi::nt::DoublePublisher targetAreaEntry; + wpi::nt::StructPublisher targetPoseEntry; + wpi::nt::DoublePublisher targetSkewEntry; - nt::DoublePublisher bestTargetPosX; - nt::DoublePublisher bestTargetPosY; + wpi::nt::DoublePublisher bestTargetPosX; + wpi::nt::DoublePublisher bestTargetPosY; - nt::IntegerTopic heartbeatTopic; - nt::IntegerPublisher heartbeatPublisher; + wpi::nt::IntegerTopic heartbeatTopic; + wpi::nt::IntegerPublisher heartbeatPublisher; - nt::DoubleArrayPublisher cameraIntrinsicsPublisher; - nt::DoubleArrayPublisher cameraDistortionPublisher; + wpi::nt::DoubleArrayPublisher cameraIntrinsicsPublisher; + wpi::nt::DoubleArrayPublisher cameraDistortionPublisher; void UpdateEntries() { - nt::PubSubOptions options; + wpi::nt::PubSubOptions options; options.periodic = 0.01; options.sendAll = true; rawBytesEntry = subTable->GetRawTopic("rawBytes") @@ -88,7 +88,8 @@ class NTTopicSet { targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish(); targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish(); targetPoseEntry = - subTable->GetStructTopic("targetPose").Publish(); + subTable->GetStructTopic("targetPose") + .Publish(); targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish(); bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish(); diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 39726b3835..4cd9447e33 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include "MultiTargetPNPResult.h" #include "PhotonTrackedTarget.h" @@ -85,8 +85,8 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { * Returns the latency in the pipeline. * @return The latency in the pipeline. */ - units::millisecond_t GetLatency() const { - return units::microsecond_t{static_cast( + wpi::units::millisecond_t GetLatency() const { + return wpi::units::microsecond_t{static_cast( metadata.publishTimestampMicros - metadata.captureTimestampMicros)}; } @@ -99,7 +99,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { * @return The timestamp in seconds or -1 if this result was not initiated * with a timestamp. */ - units::second_t GetTimestamp() const { + wpi::units::second_t GetTimestamp() const { return ntReceiveTimestamp - GetLatency(); } @@ -122,7 +122,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { int64_t SequenceID() const { return metadata.sequenceID; } /** Sets the FPGA timestamp this result was Received by robot code */ - void SetReceiveTimestamp(const units::second_t timestamp) { + void SetReceiveTimestamp(const wpi::units::second_t timestamp) { this->ntReceiveTimestamp = timestamp; } @@ -146,7 +146,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { // Since we don't trust NT time sync, keep track of when we got this packet // into robot code - units::microsecond_t ntReceiveTimestamp = -1_s; + wpi::units::microsecond_t ntReceiveTimestamp = -1_s; inline static bool HAS_WARNED = false; }; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 66edf0457e..fc9eb79935 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include "photon/struct/PhotonTrackedTargetStruct.h" @@ -121,14 +121,16 @@ class PhotonTrackedTarget : public PhotonTrackedTarget_PhotonStruct { * reprojection error is the ambiguity, which is between 0 and 1. * @return The pose of the target relative to the robot. */ - frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; } + wpi::math::Transform3d GetBestCameraToTarget() const { + return bestCameraToTarget; + } /** * Get the transform that maps camera space (X = forward, Y = left, Z = up) to * object/fiducial tag space (X forward, Y left, Z up) with the highest * reprojection error */ - frc::Transform3d GetAlternateCameraToTarget() const { + wpi::math::Transform3d GetAlternateCameraToTarget() const { return altCameraToTarget; } diff --git a/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp b/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp index ddeae34701..66041553fb 100644 --- a/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp +++ b/photon-targeting/src/main/native/jni/ConstrainedSolvepnpJNI.cpp @@ -41,12 +41,11 @@ extern "C" { * Signature: (ZI[D[D[D[D[DDD)[D */ JNIEXPORT jdoubleArray JNICALL -Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization - (JNIEnv* env, jclass, jboolean headingFree, jint nTags, - jdoubleArray cameraCal, jdoubleArray robot2camera, jdoubleArray xGuess, - jdoubleArray field2points, jdoubleArray pointObservations, jdouble gyro_θ, - jdouble gyro_error_scale_fac) -{ +Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization( + JNIEnv* env, jclass, jboolean headingFree, jint nTags, + jdoubleArray cameraCal, jdoubleArray robot2camera, jdoubleArray xGuess, + jdoubleArray field2points, jdoubleArray pointObservations, jdouble gyro_θ, + jdouble gyro_error_scale_fac) { auto cameraCalVec = convertJDoubleArray(env, cameraCal); auto robot2cameraVec = convertJDoubleArray(env, robot2camera); auto xGuessVec = convertJDoubleArray(env, xGuess); @@ -68,8 +67,8 @@ Java_org_photonvision_jni_ConstrainedSolvepnpJni_do_1optimization pointObservationsMat(pointObservationsVec.data(), 2, pointObservationsVec.size() / 2); - wpi::expected result = - constrained_solvepnp::do_optimization( + wpi::util::expected + result = constrained_solvepnp::do_optimization( headingFree, nTags, cameraCal_, robot2cameraMat, xGuessMat, field2pointsMat, pointObservationsMat, gyro_θ, gyro_error_scale_fac); diff --git a/photon-targeting/src/main/native/jni/CscoreExtras.cpp b/photon-targeting/src/main/native/jni/CscoreExtras.cpp index 9f6aa944d5..748e06c315 100644 --- a/photon-targeting/src/main/native/jni/CscoreExtras.cpp +++ b/photon-targeting/src/main/native/jni/CscoreExtras.cpp @@ -18,13 +18,13 @@ #include #include -#include +#include -#include "cscore_raw.h" #include "org_photonvision_jni_CscoreExtras.h" +#include "wpi/cs/cscore_raw.h" // from wpilib, licensed under the wpilib BSD license -using namespace wpi::java; +using namespace wpi::util::java; static JException videoEx; static const JExceptionInit exceptions[] = { {"edu/wpi/first/cscore/VideoException", &videoEx}}; @@ -115,11 +115,11 @@ Java_org_photonvision_jni_CscoreExtras_grabRawSinkFrameTimeoutLastTime (JNIEnv* env, jclass, jint sink, jlong framePtr, jdouble timeout, jlong lastFrameTimeout) { - auto* frame = reinterpret_cast(framePtr); + auto* frame = reinterpret_cast(framePtr); CS_Status status = 0; // fill frame with a copy of the latest frame from the Source - auto rv = cs::GrabSinkFrameTimeoutLastTime( + auto rv = wpi::cs::GrabSinkFrameTimeoutLastTime( static_cast(sink), *frame, timeout, lastFrameTimeout, &status); if (!CheckStatus(env, status)) { return 0; @@ -137,7 +137,7 @@ JNIEXPORT jlong JNICALL Java_org_photonvision_jni_CscoreExtras_wrapRawFrame (JNIEnv*, jclass, jlong framePtr) { - auto* frame = reinterpret_cast(framePtr); + auto* frame = reinterpret_cast(framePtr); return reinterpret_cast(new cv::Mat(frame->height, frame->width, GetCVFormat(frame->pixelFormat), @@ -153,7 +153,7 @@ JNIEXPORT jint JNICALL Java_org_photonvision_jni_CscoreExtras_getTimestampSourceNative (JNIEnv*, jclass, jlong framePtr) { - auto* frame = reinterpret_cast(framePtr); + auto* frame = reinterpret_cast(framePtr); return frame->timestampSrc; } diff --git a/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp b/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp index f0f36c3de9..edc6ddecd6 100644 --- a/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp +++ b/photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include "jni_utils.h" #include "org_photonvision_jni_QueuedFileLogger.h" @@ -35,12 +35,12 @@ struct QueuedFileLogger { explicit QueuedFileLogger(std::string_view file) : logger{file, std::bind(&QueuedFileLogger::callback, this, std::placeholders::_1)} { - // wpi::println("Watching {}", file); + // wpi::util::println("Watching {}", file); } void callback(std::string_view newline) { std::lock_guard lock{m_mutex}; - // wpi::println("FileLogger got: {}", newline); + // wpi::util::println("FileLogger got: {}", newline); m_data.insert(m_data.end(), newline.begin(), newline.end()); } diff --git a/photon-targeting/src/main/native/jni/jni_utils.h b/photon-targeting/src/main/native/jni/jni_utils.h index d94477fa4d..282a893748 100644 --- a/photon-targeting/src/main/native/jni/jni_utils.h +++ b/photon-targeting/src/main/native/jni/jni_utils.h @@ -17,15 +17,15 @@ #pragma once -#include +#include -#define CHECK_PTR(ptr) \ - if (!ptr) { \ - wpi::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ - return; \ +#define CHECK_PTR(ptr) \ + if (!ptr) { \ + wpi::util::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return; \ } -#define CHECK_PTR_RETURN(ptr, default) \ - if (!ptr) { \ - wpi::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ - return default; \ +#define CHECK_PTR_RETURN(ptr, default) \ + if (!ptr) { \ + wpi::util::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return default; \ } diff --git a/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java b/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java index 2a2e611f8a..0c78b1b505 100644 --- a/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java +++ b/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java @@ -18,10 +18,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.Arrays; import org.junit.jupiter.api.AfterAll; @@ -29,6 +25,10 @@ import org.junit.jupiter.api.Test; import org.photonvision.jni.ConstrainedSolvepnpJni; import org.photonvision.jni.LibraryLoader; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.util.Nat; +import org.wpilib.util.runtime.RuntimeLoader; public class ConstrainedSolvepnpTest { @BeforeAll diff --git a/photon-targeting/src/test/java/jni/CscoreExtrasTest.java b/photon-targeting/src/test/java/jni/CscoreExtrasTest.java index 1efb151220..9816a5e6e7 100644 --- a/photon-targeting/src/test/java/jni/CscoreExtrasTest.java +++ b/photon-targeting/src/test/java/jni/CscoreExtrasTest.java @@ -20,21 +20,21 @@ import static org.junit.jupiter.api.Assertions.fail; import static org.junit.jupiter.api.Assumptions.assumeTrue; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.CvSink; -import edu.wpi.first.cscore.CvSource; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.util.PixelFormat; -import edu.wpi.first.util.RawFrame; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import org.junit.jupiter.api.AfterAll; import org.junit.jupiter.api.BeforeAll; import org.opencv.core.Mat; import org.photonvision.jni.CscoreExtras; import org.photonvision.jni.LibraryLoader; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.util.PixelFormat; +import org.wpilib.util.RawFrame; +import org.wpilib.util.runtime.RuntimeLoader; +import org.wpilib.vision.camera.CameraServerJNI; +import org.wpilib.vision.camera.CvSink; +import org.wpilib.vision.camera.CvSource; +import org.wpilib.vision.camera.UsbCamera; +import org.wpilib.vision.stream.CameraServer; public class CscoreExtrasTest { @BeforeAll diff --git a/photon-targeting/src/test/java/jni/FileLoggerTest.java b/photon-targeting/src/test/java/jni/FileLoggerTest.java index fe2f129f88..8aa29b0d67 100644 --- a/photon-targeting/src/test/java/jni/FileLoggerTest.java +++ b/photon-targeting/src/test/java/jni/FileLoggerTest.java @@ -20,8 +20,6 @@ import static org.junit.jupiter.api.Assertions.fail; import static org.junit.jupiter.api.Assumptions.assumeTrue; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import org.junit.jupiter.api.AfterAll; import org.junit.jupiter.api.BeforeAll; @@ -29,6 +27,8 @@ import org.photonvision.common.hardware.Platform; import org.photonvision.jni.LibraryLoader; import org.photonvision.jni.QueuedFileLogger; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.util.runtime.RuntimeLoader; public class FileLoggerTest { @BeforeAll diff --git a/photon-targeting/src/test/java/net/TimeSyncTest.java b/photon-targeting/src/test/java/net/TimeSyncTest.java index 2234ee50ab..fbbd4f1f25 100644 --- a/photon-targeting/src/test/java/net/TimeSyncTest.java +++ b/photon-targeting/src/test/java/net/TimeSyncTest.java @@ -17,8 +17,6 @@ package net; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import org.junit.jupiter.api.AfterAll; import org.junit.jupiter.api.BeforeAll; @@ -26,6 +24,8 @@ import org.photonvision.jni.LibraryLoader; import org.photonvision.jni.TimeSyncClient; import org.photonvision.jni.TimeSyncServer; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.util.runtime.RuntimeLoader; public class TimeSyncTest { @BeforeAll diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java index 4b1b140d16..b531f54816 100644 --- a/photon-targeting/src/test/java/org/photonvision/PacketTest.java +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -19,7 +19,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.*; import java.util.List; import java.util.Optional; import org.junit.jupiter.api.Test; @@ -29,6 +28,7 @@ import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +import org.wpilib.math.geometry.*; class PacketTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java index 80f757efcc..a7c92e1c6f 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java @@ -20,11 +20,11 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; public class MultiTargetPNPResultTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java index d7de00af29..ebd36802fb 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java @@ -20,9 +20,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; public class PNPResultTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 95d9506c3e..b520f33ba5 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -20,12 +20,12 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import java.util.Optional; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; public class PhotonPipelineResultTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index 15a44cb2fd..ed03aa2301 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -20,11 +20,11 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; public class PhotonTrackedTargetTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java index 208f4f19b3..a8bc77de57 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java @@ -19,13 +19,13 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PnpResult; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; public class MultiTargetPNPResultProtoTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java index cc857a8c61..bf038b204b 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java @@ -19,10 +19,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import org.junit.jupiter.api.Test; import org.photonvision.targeting.PnpResult; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; public class PNPResultProtoTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java index a60dddbed8..80150e65ff 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -19,13 +19,13 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import java.util.Optional; import org.junit.jupiter.api.Test; import org.photonvision.targeting.*; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; public class PhotonPipelineResultProtoTest { @Test diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java index aba75576cb..be2a8df17a 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java @@ -19,14 +19,14 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTargetProtoTest { diff --git a/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp b/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp index c2e66682a6..9a22daa4b4 100644 --- a/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp +++ b/photon-targeting/src/test/native/cpp/CasadiWrapperTest.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include "photon/constrained_solvepnp/wrap/casadi_wrapper.h" @@ -159,12 +159,12 @@ void print_cost(casadi_real robot_x, casadi_real robot_y, x_guess << robot_x, robot_y, robot_theta; for (int i = 0; i < 200; i++) { - auto start = wpi::Now(); + auto start = wpi::util::Now(); auto x_out = constrained_solvepnp::do_optimization( true, TAG_COUNT, constrained_solvepnp::CameraCalibration{fx, fy, cx, cy}, robot2camera, x_guess, field2points, point_observations, 0, 0); - auto end = wpi::Now(); + auto end = wpi::util::Now(); wpi::println("{},{},{}", i, static_cast(x_out), end - start); wpi::println( diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 94cb707e13..923d19713c 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include "gtest/gtest.h" #include "photon/targeting/MultiTargetPNPResult.h" @@ -34,8 +34,8 @@ using namespace photon; TEST(PacketTest, PnpResult) { PnpResult result{}; - result.best = {1_m, 2_m, 3_m, frc::Rotation3d{6_deg, 7_deg, 12_deg}}; - result.alt = {8_m, 2_m, 1_m, frc::Rotation3d{0_deg, 1_deg, 88_deg}}; + result.best = {1_m, 2_m, 3_m, wpi::math::Rotation3d{6_deg, 7_deg, 12_deg}}; + result.alt = {8_m, 2_m, 1_m, wpi::math::Rotation3d{0_deg, 1_deg, 88_deg}}; // determined by throwing a few D20s result.bestReprojErr = 7; result.altReprojErr = 11; @@ -69,10 +69,10 @@ TEST(PacketTest, PnpResult) { // -1, // -1, // -1.0, -// frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), -// frc::Rotation3d(1_rad, 2_rad, 3_rad)), -// frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), -// frc::Rotation3d(1_rad, 2_rad, 3_rad)), +// wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), +// wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), +// wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), +// wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), // -1, // {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, // {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; @@ -98,10 +98,10 @@ TEST(PacketTest, PhotonPipelineResult) { std::vector targets{ PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1.0f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), -1.0, std::vector{ TargetCorner{1., 2.}, TargetCorner{3.0, 4.0}, @@ -111,10 +111,10 @@ TEST(PacketTest, PhotonPipelineResult) { TargetCorner{5., 6.}, TargetCorner{7.0, 8.0}}}, PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, -1, -1, -1.0f, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), + wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), + wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), -1.0, std::vector{ TargetCorner{1.0, 2.0}, TargetCorner{3.0, 4.0}, @@ -124,10 +124,10 @@ TEST(PacketTest, PhotonPipelineResult) { TargetCorner{5.0, 6.0}, TargetCorner{7.0, 8.0}}}}; MultiTargetPNPResult mtResult{ - PnpResult{frc::Transform3d{1_m, 2_m, 3_m, - frc::Rotation3d{6_deg, 7_deg, 12_deg}}, - frc::Transform3d{8_m, 2_m, 1_m, - frc::Rotation3d{0_deg, 1_deg, 88_deg}}, + PnpResult{wpi::math::Transform3d{ + 1_m, 2_m, 3_m, wpi::math::Rotation3d{6_deg, 7_deg, 12_deg}}, + wpi::math::Transform3d{ + 8_m, 2_m, 1_m, wpi::math::Rotation3d{0_deg, 1_deg, 88_deg}}, // determined by throwing a few D20s 17.0, 22.33, 2.54}, std::vector{8, 7, 11, 22, 59, 40}}; @@ -143,7 +143,7 @@ TEST(PacketTest, PhotonPipelineResult) { auto t3 = std::chrono::steady_clock::now(); EXPECT_EQ(result2, b2); - wpi::println( + wpi::util::println( "Pack {} unpack {} packet length {}", std::chrono::duration_cast(t2 - t1).count(), std::chrono::duration_cast(t3 - t2).count(), diff --git a/photon-targeting/src/test/native/cpp/main.cpp b/photon-targeting/src/test/native/cpp/main.cpp index 47adcce931..89d0cd5c16 100644 --- a/photon-targeting/src/test/native/cpp/main.cpp +++ b/photon-targeting/src/test/native/cpp/main.cpp @@ -15,7 +15,7 @@ * along with this program. If not, see . */ -#include +#include #include "gtest/gtest.h" diff --git a/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp index 56f80310a2..4d550607b8 100644 --- a/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp +++ b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp @@ -16,10 +16,10 @@ */ #include -#include #include #include -#include +#include +#include TEST(TimeSyncProtocolTest, Smoketest) { using namespace wpi::tsp; @@ -34,7 +34,7 @@ TEST(TimeSyncProtocolTest, Smoketest) { for (int i = 0; i < 10; i++) { std::this_thread::sleep_for(100ms); TimeSyncClient::Metadata m = client.GetMetadata(); - wpi::println("Offset={} rtt={}", m.offset, m.rtt2); + wpi::util::println("Offset={} rtt={}", m.offset, m.rtt2); } server.Stop(); diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp index c5ac66f4c4..387b9897cf 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp @@ -24,9 +24,9 @@ #include "Robot.h" -#include -#include #include +#include +#include void Robot::RobotInit() {} @@ -48,7 +48,7 @@ void Robot::AutonomousPeriodic() {} void Robot::AutonomousExit() {} void Robot::TeleopInit() { - frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}}; drivetrain.ResetPose(pose, true); } @@ -62,8 +62,8 @@ void Robot::TeleopPeriodic() { -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; bool targetVisible = false; - units::degree_t targetYaw = 0.0_deg; - units::meter_t targetRange = 0.0_m; + wpi::units::degree_t targetYaw = 0.0_deg; + wpi::units::meter_t targetRange = 0.0_m; auto results = camera.GetAllUnreadResults(); if (results.size() > 0) { // Camera processed a new frame since last @@ -74,12 +74,12 @@ void Robot::TeleopPeriodic() { for (auto& target : result.GetTargets()) { if (target.GetFiducialId() == 7) { // Found Tag 7, record its information - targetYaw = units::degree_t{target.GetYaw()}; + targetYaw = wpi::units::degree_t{target.GetYaw()}; targetRange = photon::PhotonUtils::CalculateDistanceToTarget( 0.5_m, // Measured with a tape measure, or in CAD 1.435_m, // From 2024 game manual for ID 7 -30.0_deg, // Measured with a protractor, or in CAD - units::degree_t{target.GetPitch()}); + wpi::units::degree_t{target.GetPitch()}); targetVisible = true; } } @@ -114,19 +114,19 @@ void Robot::SimulationPeriodic() { drivetrain.SimulationPeriodic(); vision.SimPeriodic(drivetrain.GetSimPose()); - frc::Field2d& debugField = vision.GetSimDebugField(); + wpi::Field2d& debugField = vision.GetSimDebugField(); debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); debugField.GetObject("EstimatedRobotModules") ->SetPoses(drivetrain.GetModulePoses()); - units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); - units::volt_t loadedBattVolts = - frc::sim::BatterySim::Calculate({totalCurrent}); + wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + wpi::units::volt_t loadedBattVolts = + wpi::sim::BatterySim::Calculate({totalCurrent}); // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } +int main() { return wpi::StartRobot(); } #endif diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp index 40ba22f6b4..4703597d09 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp @@ -26,18 +26,19 @@ #include -#include -#include +#include +#include SwerveDrive::SwerveDrive() : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), - frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), // gyroSim(gyro), - swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kDriveGearRatio, - constants::Swerve::kWheelDiameter / 2, - constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kSteerGearRatio, kinematics) {} + swerveDriveSim( + constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF, + wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio, + kinematics) {} void SwerveDrive::Periodic() { for (auto& currentModule : swerveMods) { @@ -47,27 +48,30 @@ void SwerveDrive::Periodic() { poseEstimator.Update(GetGyroYaw(), GetModulePositions()); } -void SwerveDrive::Drive(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega) { - frc::ChassisSpeeds newChassisSpeeds = - frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); +void SwerveDrive::Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega) { + wpi::math::ChassisSpeeds newChassisSpeeds = + wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, + GetHeading()); SetChassisSpeeds(newChassisSpeeds, true, false); } -void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, - bool openLoop, bool steerInPlace) { +void SwerveDrive::SetChassisSpeeds( + const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop, + bool steerInPlace) { SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, steerInPlace); this->targetChassisSpeeds = newChassisSpeeds; } void SwerveDrive::SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace) { - std::array desaturatedStates = desiredStates; - frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( - static_cast*>(&desaturatedStates), + const std::array& desiredStates, + bool openLoop, bool steerInPlace) { + std::array desaturatedStates = desiredStates; + wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>( + &desaturatedStates), constants::Swerve::kMaxLinearSpeed); for (int i = 0; i < swerveMods.size(); i++) { swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); @@ -76,7 +80,7 @@ void SwerveDrive::SetModuleStates( void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } -void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { +void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) { if (resetSimPose) { swerveDriveSim.Reset(pose, false); for (int i = 0; i < swerveMods.size(); i++) { @@ -89,20 +93,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); } -frc::Pose2d SwerveDrive::GetPose() const { +wpi::math::Pose2d SwerveDrive::GetPose() const { return poseEstimator.GetEstimatedPosition(); } -frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } +wpi::math::Rotation2d SwerveDrive::GetHeading() const { + return GetPose().Rotation(); +} -frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } +wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const { + return gyro.GetRotation2d(); +} -frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { +wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { return kinematics.ToChassisSpeeds(GetModuleStates()); } -std::array SwerveDrive::GetModuleStates() const { - std::array moduleStates; +std::array SwerveDrive::GetModuleStates() + const { + std::array moduleStates; moduleStates[0] = swerveMods[0].GetState(); moduleStates[1] = swerveMods[1].GetState(); moduleStates[2] = swerveMods[2].GetState(); @@ -110,9 +119,9 @@ std::array SwerveDrive::GetModuleStates() const { return moduleStates; } -std::array SwerveDrive::GetModulePositions() +std::array SwerveDrive::GetModulePositions() const { - std::array modulePositions; + std::array modulePositions; modulePositions[0] = swerveMods[0].GetPosition(); modulePositions[1] = swerveMods[1].GetPosition(); modulePositions[2] = swerveMods[2].GetPosition(); @@ -120,11 +129,11 @@ std::array SwerveDrive::GetModulePositions() return modulePositions; } -std::array SwerveDrive::GetModulePoses() const { - std::array modulePoses; +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; for (int i = 0; i < swerveMods.size(); i++) { const SwerveModule& module = swerveMods[i]; - modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{ module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); } return modulePoses; @@ -132,24 +141,24 @@ std::array SwerveDrive::GetModulePoses() const { void SwerveDrive::Log() { std::string table = "Drive/"; - frc::Pose2d pose = GetPose(); - frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); - frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); - frc::SmartDashboard::PutNumber(table + "Heading", + wpi::math::Pose2d pose = GetPose(); + wpi::SmartDashboard::PutNumber(table + "X", pose.X().to()); + wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + wpi::SmartDashboard::PutNumber(table + "Heading", pose.Rotation().Degrees().to()); - frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); - frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); - frc::SmartDashboard::PutNumber(table + "Target VX", + chassisSpeeds.omega.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "Target VY", + wpi::SmartDashboard::PutNumber(table + "Target VY", targetChassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Target Omega Degrees", - targetChassisSpeeds.omega.convert() + targetChassisSpeeds.omega.convert() .to()); for (auto& module : swerveMods) { @@ -158,8 +167,8 @@ void SwerveDrive::Log() { } void SwerveDrive::SimulationPeriodic() { - std::array driveInputs; - std::array steerInputs; + std::array driveInputs; + std::array steerInputs; for (int i = 0; i < swerveMods.size(); i++) { driveInputs[i] = swerveMods[i].GetDriveVoltage(); steerInputs[i] = swerveMods[i].GetSteerVoltage(); @@ -167,26 +176,26 @@ void SwerveDrive::SimulationPeriodic() { swerveDriveSim.SetDriveInputs(driveInputs); swerveDriveSim.SetSteerInputs(steerInputs); - swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod); auto driveStates = swerveDriveSim.GetDriveStates(); auto steerStates = swerveDriveSim.GetSteerStates(); totalCurrentDraw = 0_A; - std::array driveCurrents = + std::array driveCurrents = swerveDriveSim.GetDriveCurrentDraw(); for (const auto& current : driveCurrents) { totalCurrentDraw += current; } - std::array steerCurrents = + std::array steerCurrents = swerveDriveSim.GetSteerCurrentDraw(); for (const auto& current : steerCurrents) { totalCurrentDraw += current; } for (int i = 0; i < swerveMods.size(); i++) { - units::meter_t drivePos{driveStates[i](0, 0)}; - units::meters_per_second_t driveRate{driveStates[i](1, 0)}; - units::radian_t steerPos{steerStates[i](0, 0)}; - units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + wpi::units::meter_t drivePos{driveStates[i](0, 0)}; + wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + wpi::units::radian_t steerPos{steerStates[i](0, 0)}; + wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)}; swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } @@ -194,6 +203,8 @@ void SwerveDrive::SimulationPeriodic() { // gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); } -frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } +wpi::math::Pose2d SwerveDrive::GetSimPose() const { + return swerveDriveSim.GetPose(); +} -units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp index 3cfc9a6d45..cdcb81acd5 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -24,8 +24,8 @@ #include "subsystems/SwerveDriveSim.h" -#include -#include +#include +#include template int sgn(T val) { @@ -33,14 +33,14 @@ int sgn(T val) { } SwerveDriveSim::SwerveDriveSim( - const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : SwerveDriveSim( - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -driveFF.GetKv().to() / driveFF.GetKa().to()) .finished(), @@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim( (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), Eigen::Matrix{0.0, 0.0}}, driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius, - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -steerFF.GetKv().to() / steerFF.GetKa().to()) .finished(), @@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), driveMotor(driveMotor), @@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim( kinematics(kinematics) {} void SwerveDriveSim::SetDriveInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < driveInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } void SwerveDriveSim::SetSteerInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < steerInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } @@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs( Eigen::Matrix SwerveDriveSim::CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS) { + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS) { auto Ax = discA * x; double nextStateVel = Ax(1, 0); double inputToStop = nextStateVel / -discB(1, 0); @@ -129,18 +129,18 @@ Eigen::Matrix SwerveDriveSim::CalculateX( return retVal; } -void SwerveDriveSim::Update(units::second_t dt) { +void SwerveDriveSim::Update(wpi::units::second_t dt) { Eigen::Matrix driveDiscA; Eigen::Matrix driveDiscB; - frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, - &driveDiscB); + wpi::math::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); Eigen::Matrix steerDiscA; Eigen::Matrix steerDiscB; - frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, - &steerDiscB); + wpi::math::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); - std::array moduleDeltas; + std::array moduleDeltas; for (int i = 0; i < numModules; i++) { double prevDriveStatePos = driveStates[i](0, 0); @@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) { steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], steerInputs[i], steerKs); double currentSteerStatePos = steerStates[i](0, 0); - moduleDeltas[i] = frc::SwerveModulePosition{ - units::meter_t{currentDriveStatePos - prevDriveStatePos}, - frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + moduleDeltas[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos}, + wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}}; } - frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + wpi::math::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); pose = pose.Exp(twist); omega = twist.dtheta / dt; } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) { this->pose = pose; if (!preserveMotion) { for (int i = 0; i < numModules; i++) { @@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { } } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, @@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; } -frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } +wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; } -std::array +std::array SwerveDriveSim::GetModulePositions() const { - std::array positions; + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return positions; } -std::array -SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, - units::radian_t steerStdDev) { - std::array positions; +std::array +SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev) { + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)} + + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)} + randDist(generator) * driveStdDev, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + - randDist(generator) * steerStdDev}}; + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; } return positions; } -std::array +std::array SwerveDriveSim::GetModuleStates() { - std::array states; + std::array states; for (int i = 0; i < numModules; i++) { - states[i] = frc::SwerveModuleState{ - units::meters_per_second_t{driveStates[i](1, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + states[i] = wpi::math::SwerveModuleState{ + wpi::units::meters_per_second_t{driveStates[i](1, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return states; } @@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } -units::ampere_t SwerveDriveSim::GetCurrentDraw( - const frc::DCMotor& motor, units::radians_per_second_t velocity, - units::volt_t inputVolts, units::volt_t batteryVolts) const { - units::volt_t effVolts = inputVolts - velocity / motor.Kv; +wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( + const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const { + wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv; if (inputVolts >= 0_V) { effVolts = std::clamp(effVolts, 0_V, inputVolts); } else { @@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() +std::array SwerveDriveSim::GetDriveCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / driveWheelRadius.to(); currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], - frc::RobotController::GetBatteryVoltage()); + wpi::RobotController::GetBatteryVoltage()); } return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() +std::array SwerveDriveSim::GetSteerCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; // TODO: If uncommented we get huge current values.. Not sure how to fix // atm. :( currents[i] = 20_A; // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], - // frc::RobotController::GetBatteryVoltage()); + // wpi::RobotController::GetBatteryVoltage()); } return currents; } -units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { - units::ampere_t total{0}; +wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + wpi::units::ampere_t total{0}; for (const auto& val : GetDriveCurrentDraw()) { total += val; } diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp index 4b0e27da6d..3bfa786b22 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp @@ -26,17 +26,17 @@ #include -#include -#include -#include +#include +#include +#include SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) : moduleConstants(consts), - driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), - driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA, moduleConstants.driveEncoderB}), - steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), - steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA, moduleConstants.steerEncoderB}), driveEncoderSim(driveEncoder), steerEncoderSim(steerEncoder) { @@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) } void SwerveModule::Periodic() { - units::volt_t steerPID = units::volt_t{ + wpi::units::volt_t steerPID = wpi::units::volt_t{ steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), desiredState.angle.Radians().to())}; steerMotor.SetVoltage(steerPID); - units::volt_t driveFF = + wpi::units::volt_t driveFF = constants::Swerve::kDriveFF.Calculate(desiredState.speed); - units::volt_t drivePID{0}; + wpi::units::volt_t drivePID{0}; if (!openLoop) { - drivePID = units::volt_t{drivePIDController.Calculate( + drivePID = wpi::units::volt_t{drivePIDController.Calculate( driveEncoder.GetRate(), desiredState.speed.to())}; } driveMotor.SetVoltage(driveFF + drivePID); } -void SwerveModule::SetDesiredState(frc::SwerveModuleState newState, +void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, bool shouldBeOpenLoop, bool steerInPlace) { - frc::Rotation2d currentRotation = GetAbsoluteHeading(); + wpi::math::Rotation2d currentRotation = GetAbsoluteHeading(); newState.Optimize(currentRotation); desiredState = newState; } -frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; } -frc::SwerveModuleState SwerveModule::GetState() const { - return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, - GetAbsoluteHeading()}; +wpi::math::SwerveModuleState SwerveModule::GetState() const { + return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; } -frc::SwerveModulePosition SwerveModule::GetPosition() const { - return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, - GetAbsoluteHeading()}; +wpi::math::SwerveModulePosition SwerveModule::GetPosition() const { + return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; } -units::volt_t SwerveModule::GetDriveVoltage() const { - return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::volt_t SwerveModule::GetSteerVoltage() const { - return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::ampere_t SwerveModule::GetDriveCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const { return driveCurrentSim; } -units::ampere_t SwerveModule::GetSteerCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const { return steerCurrentSim; } @@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { } void SwerveModule::Log() { - frc::SwerveModuleState state = GetState(); + wpi::math::SwerveModuleState state = GetState(); std::string table = "Module " + std::to_string(moduleConstants.moduleNum) + "/"; - frc::SmartDashboard::PutNumber(table + "Steer Degrees", - frc::AngleModulus(state.angle.Radians()) - .convert() + wpi::SmartDashboard::PutNumber(table + "Steer Degrees", + wpi::math::AngleModulus(state.angle.Radians()) + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Steer Target Degrees", - units::radian_t{steerPIDController.GetSetpoint()} - .convert() + wpi::units::radian_t{steerPIDController.GetSetpoint()} + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Feet", - state.speed.convert().to()); - frc::SmartDashboard::PutNumber( + state.speed.convert().to()); + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Target Feet", - desiredState.speed.convert().to()); - frc::SmartDashboard::PutNumber(table + "Drive Current", + desiredState.speed.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Drive Current", driveCurrentSim.to()); - frc::SmartDashboard::PutNumber(table + "Steer Current", + wpi::SmartDashboard::PutNumber(table + "Steer Current", steerCurrentSim.to()); } void SwerveModule::SimulationUpdate( - units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent) { + wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); driveEncoderSim.SetRate(driveEncoderRate.to()); driveCurrentSim = driveCurrent; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h index 70a5d74bd5..1e27703dfa 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h @@ -26,25 +26,26 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace constants { namespace Vision { inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; -inline const frc::Transform3d kRobotToCam{ - frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, - frc::Rotation3d{0_rad, -30_deg, 0_rad}}; -inline const frc::AprilTagFieldLayout kTagLayout{ - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)}; +inline const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m}, + wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}}; +inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{ + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::kDefaultField)}; inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; @@ -52,23 +53,23 @@ inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; namespace Swerve { using namespace units; -inline constexpr units::meter_t kTrackWidth{18.5_in}; -inline constexpr units::meter_t kTrackLength{18.5_in}; -inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; -inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; -inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; -inline constexpr units::meter_t kWheelDiameter{4_in}; -inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * +inline constexpr wpi::units::meter_t kTrackWidth{18.5_in}; +inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; +inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; +inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; -inline constexpr units::meter_t kDriveDistPerPulse = +inline constexpr wpi::units::meter_t kDriveDistPerPulse = kWheelCircumference / 1024.0 / kDriveGearRatio; -inline constexpr units::radian_t kSteerRadPerPulse = - units::radian_t{2 * std::numbers::pi} / 1024.0; +inline constexpr wpi::units::radian_t kSteerRadPerPulse = + wpi::units::radian_t{2 * std::numbers::pi} / 1024.0; inline constexpr double kDriveKP = 1.0; inline constexpr double kDriveKI = 0.0; @@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25; using namespace units; -inline const frc::SimpleMotorFeedforward kDriveFF{ +inline const wpi::math::SimpleMotorFeedforward kDriveFF{ 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; -inline const frc::SimpleMotorFeedforward kSteerFF{ +inline const wpi::math::SimpleMotorFeedforward kSteerFF{ 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; struct ModuleConstants { @@ -96,12 +97,12 @@ struct ModuleConstants { const int steerEncoderA; const int steerEncoderB; const double angleOffset; - const frc::Translation2d centerOffset; + const wpi::math::Translation2d centerOffset; ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, units::meter_t xOffset, - units::meter_t yOffset) + int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, + wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), @@ -110,7 +111,7 @@ struct ModuleConstants { steerEncoderA(steerEncoderA), steerEncoderB(steerEncoderB), angleOffset(angleOffset), - centerOffset(frc::Translation2d{xOffset, yOffset}) {} + centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {} }; inline const ModuleConstants FL_CONSTANTS{ diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h index 34ffe8f89b..c14770a4b8 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h @@ -24,15 +24,15 @@ #pragma once -#include -#include #include +#include +#include #include "Constants.h" #include "VisionSim.h" #include "subsystems/SwerveDrive.h" -class Robot : public frc::TimedRobot { +class Robot : public wpi::TimedRobot { public: void RobotInit() override; void RobotPeriodic() override; @@ -54,7 +54,7 @@ class Robot : public frc::TimedRobot { photon::PhotonCamera camera{constants::Vision::kCameraName}; SwerveDrive drivetrain{}; VisionSim vision{&camera}; - frc::XboxController controller{0}; + wpi::XboxController controller{0}; static constexpr auto VISION_TURN_kP = 0.01; static constexpr auto VISION_DES_ANGLE = 0.0_deg; static constexpr auto VISION_STRAFE_kP = 0.5; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h b/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h index 3520165299..fffc5b1d3b 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h @@ -27,28 +27,28 @@ #include #include -#include -#include #include #include #include #include #include #include +#include +#include #include "Constants.h" class VisionSim { public: explicit VisionSim(photon::PhotonCamera* camera) { - if (frc::RobotBase::IsSimulation()) { + if (wpi::RobotBase::IsSimulation()) { visionSim = std::make_unique("main"); visionSim->AddAprilTags(constants::Vision::kTagLayout); cameraProp = std::make_unique(); - cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg}); + cameraProp->SetCalibration(320, 240, wpi::math::Rotation2d{90_deg}); cameraProp->SetCalibError(.35, .10); cameraProp->SetFPS(70_Hz); cameraProp->SetAvgLatency(30_ms); @@ -64,17 +64,17 @@ class VisionSim { photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } - void SimPeriodic(frc::Pose2d robotSimPose) { + void SimPeriodic(wpi::math::Pose2d robotSimPose) { visionSim->Update(robotSimPose); } - void ResetSimPose(frc::Pose2d pose) { - if (frc::RobotBase::IsSimulation()) { + void ResetSimPose(wpi::math::Pose2d pose) { + if (wpi::RobotBase::IsSimulation()) { visionSim->ResetRobotPose(pose); } } - frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } private: std::unique_ptr visionSim; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h index 81e3d47bd9..6a68e88150 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -24,9 +24,9 @@ #pragma once -#include -#include -#include +#include +#include +#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -35,26 +35,26 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega); - void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega); + void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); void SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace); + const std::array& desiredStates, + bool openLoop, bool steerInPlace); void Stop(); - void ResetPose(const frc::Pose2d& pose, bool resetSimPose); - frc::Pose2d GetPose() const; - frc::Rotation2d GetHeading() const; - frc::Rotation2d GetGyroYaw() const; - frc::ChassisSpeeds GetChassisSpeeds() const; - std::array GetModuleStates() const; - std::array GetModulePositions() const; - std::array GetModulePoses() const; + void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose); + wpi::math::Pose2d GetPose() const; + wpi::math::Rotation2d GetHeading() const; + wpi::math::Rotation2d GetGyroYaw() const; + wpi::math::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; void Log(); void SimulationPeriodic(); - frc::Pose2d GetSimPose() const; - units::ampere_t GetCurrentDraw() const; + wpi::math::Pose2d GetSimPose() const; + wpi::units::ampere_t GetCurrentDraw() const; private: std::array swerveMods{ @@ -62,18 +62,18 @@ class SwerveDrive { SwerveModule{constants::Swerve::FR_CONSTANTS}, SwerveModule{constants::Swerve::BL_CONSTANTS}, SwerveModule{constants::Swerve::BR_CONSTANTS}}; - frc::SwerveDriveKinematics<4> kinematics{ + wpi::math::SwerveDriveKinematics<4> kinematics{ swerveMods[0].GetModuleConstants().centerOffset, swerveMods[1].GetModuleConstants().centerOffset, swerveMods[2].GetModuleConstants().centerOffset, swerveMods[3].GetModuleConstants().centerOffset, }; - frc::OnboardIMU gyro{MountOrientation::kFlat}; - frc::SwerveDrivePoseEstimator<4> poseEstimator; - frc::ChassisSpeeds targetChassisSpeeds{}; + wpi::OnboardIMU gyro{MountOrientation::kFlat}; + wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; + wpi::math::ChassisSpeeds targetChassisSpeeds{}; // TODO(Jade) onboard imu doesn't have sim yet - // frc::sim::ADXRS450_GyroSim gyroSim; + // wpi::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; - units::ampere_t totalCurrentDraw{0}; + wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h index c1cee3e6f1..5ace0164dd 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h @@ -26,77 +26,80 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include static constexpr int numModules{4}; class SwerveDriveSim { public: - SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, - units::volt_t driveKs, const frc::DCMotor& driveMotor, - double driveGearing, units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, - units::volt_t steerKs, const frc::DCMotor& steerMotor, - double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - void SetDriveInputs(const std::array& inputs); - void SetSteerInputs(const std::array& inputs); + SwerveDriveSim( + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + SwerveDriveSim( + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); static Eigen::Matrix CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS); - void Update(units::second_t dt); - void Reset(const frc::Pose2d& pose, bool preserveMotion); - void Reset(const frc::Pose2d& pose, + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS); + void Update(wpi::units::second_t dt); + void Reset(const wpi::math::Pose2d& pose, bool preserveMotion); + void Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, numModules>& moduleSteerStates); - frc::Pose2d GetPose() const; - std::array GetModulePositions() const; - std::array GetNoisyModulePositions( - units::meter_t driveStdDev, units::radian_t steerStdDev); - std::array GetModuleStates(); + wpi::math::Pose2d GetPose() const; + std::array GetModulePositions() + const; + std::array + GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev); + std::array GetModuleStates(); std::array, numModules> GetDriveStates() const; std::array, numModules> GetSteerStates() const; - units::radians_per_second_t GetOmega() const; - units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, - units::radians_per_second_t velocity, - units::volt_t inputVolts, - units::volt_t batteryVolts) const; - std::array GetDriveCurrentDraw() const; - std::array GetSteerCurrentDraw() const; - units::ampere_t GetTotalCurrentDraw() const; + wpi::units::radians_per_second_t GetOmega() const; + wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + wpi::units::ampere_t GetTotalCurrentDraw() const; private: std::random_device rd{}; std::mt19937 generator{rd()}; std::normal_distribution randDist{0.0, 1.0}; - const frc::LinearSystem<2, 1, 2> drivePlant; - const units::volt_t driveKs; - const frc::DCMotor driveMotor; + const wpi::math::LinearSystem<2, 1, 2> drivePlant; + const wpi::units::volt_t driveKs; + const wpi::math::DCMotor driveMotor; const double driveGearing; - const units::meter_t driveWheelRadius; - const frc::LinearSystem<2, 1, 2> steerPlant; - const units::volt_t steerKs; - const frc::DCMotor steerMotor; + const wpi::units::meter_t driveWheelRadius; + const wpi::math::LinearSystem<2, 1, 2> steerPlant; + const wpi::units::volt_t steerKs; + const wpi::math::DCMotor steerMotor; const double steerGearing; - const frc::SwerveDriveKinematics kinematics; - std::array driveInputs{}; + const wpi::math::SwerveDriveKinematics kinematics; + std::array driveInputs{}; std::array, numModules> driveStates{}; - std::array steerInputs{}; + std::array steerInputs{}; std::array, numModules> steerStates{}; - frc::Pose2d pose{frc::Pose2d{}}; - units::radians_per_second_t omega{0}; + wpi::math::Pose2d pose{wpi::math::Pose2d{}}; + wpi::units::radians_per_second_t omega{0}; }; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h index 9c07ab426d..cb9a9b2df4 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h @@ -24,13 +24,13 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "Constants.h" @@ -38,44 +38,44 @@ class SwerveModule { public: explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); void Periodic(); - void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop, - bool steerInPlace); - frc::Rotation2d GetAbsoluteHeading() const; - frc::SwerveModuleState GetState() const; - frc::SwerveModulePosition GetPosition() const; - units::volt_t GetDriveVoltage() const; - units::volt_t GetSteerVoltage() const; - units::ampere_t GetDriveCurrentSim() const; - units::ampere_t GetSteerCurrentSim() const; + void SetDesiredState(wpi::math::SwerveModuleState newState, + bool shouldBeOpenLoop, bool steerInPlace); + wpi::math::Rotation2d GetAbsoluteHeading() const; + wpi::math::SwerveModuleState GetState() const; + wpi::math::SwerveModulePosition GetPosition() const; + wpi::units::volt_t GetDriveVoltage() const; + wpi::units::volt_t GetSteerVoltage() const; + wpi::units::ampere_t GetDriveCurrentSim() const; + wpi::units::ampere_t GetSteerCurrentSim() const; constants::Swerve::ModuleConstants GetModuleConstants() const; void Log(); - void SimulationUpdate(units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, - units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent); + void SimulationUpdate(wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent); private: const constants::Swerve::ModuleConstants moduleConstants; - frc::PWMSparkMax driveMotor; - frc::Encoder driveEncoder; - frc::PWMSparkMax steerMotor; - frc::Encoder steerEncoder; + wpi::PWMSparkMax driveMotor; + wpi::Encoder driveEncoder; + wpi::PWMSparkMax steerMotor; + wpi::Encoder steerEncoder; - frc::SwerveModuleState desiredState{}; + wpi::math::SwerveModuleState desiredState{}; bool openLoop{false}; - frc::PIDController drivePIDController{constants::Swerve::kDriveKP, - constants::Swerve::kDriveKI, - constants::Swerve::kDriveKD}; - frc::PIDController steerPIDController{constants::Swerve::kSteerKP, - constants::Swerve::kSteerKI, - constants::Swerve::kSteerKD}; + wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; - frc::sim::EncoderSim driveEncoderSim; - units::ampere_t driveCurrentSim{0}; - frc::sim::EncoderSim steerEncoderSim; - units::ampere_t steerCurrentSim{0}; + wpi::sim::EncoderSim driveEncoderSim; + wpi::units::ampere_t driveCurrentSim{0}; + wpi::sim::EncoderSim steerEncoderSim; + wpi::units::ampere_t steerCurrentSim{0}; }; diff --git a/photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp b/photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp index 031d1ce96b..4fcf3fefd8 100644 --- a/photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp +++ b/photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include +#include #include "gtest/gtest.h" diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 832884bc83..acb89dc9bc 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -24,8 +24,8 @@ #include "Robot.h" -#include -#include +#include +#include void Robot::RobotInit() {} @@ -47,7 +47,7 @@ void Robot::AutonomousPeriodic() {} void Robot::AutonomousExit() {} void Robot::TeleopInit() { - frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}}; drivetrain.ResetPose(pose, true); } @@ -105,19 +105,19 @@ void Robot::SimulationPeriodic() { drivetrain.SimulationPeriodic(); vision.SimPeriodic(drivetrain.GetSimPose()); - frc::Field2d& debugField = vision.GetSimDebugField(); + wpi::Field2d& debugField = vision.GetSimDebugField(); debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); debugField.GetObject("EstimatedRobotModules") ->SetPoses(drivetrain.GetModulePoses()); - units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); - units::volt_t loadedBattVolts = - frc::sim::BatterySim::Calculate({totalCurrent}); + wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + wpi::units::volt_t loadedBattVolts = + wpi::sim::BatterySim::Calculate({totalCurrent}); // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } +int main() { return wpi::StartRobot(); } #endif diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp index 4602def8a9..2d594a992e 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp @@ -26,18 +26,19 @@ #include -#include -#include +#include +#include SwerveDrive::SwerveDrive() : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), - frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), gyroSim(gyro), - swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kDriveGearRatio, - constants::Swerve::kWheelDiameter / 2, - constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kSteerGearRatio, kinematics) {} + swerveDriveSim( + constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF, + wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio, + kinematics) {} void SwerveDrive::Periodic() { for (auto& currentModule : swerveMods) { @@ -47,27 +48,30 @@ void SwerveDrive::Periodic() { poseEstimator.Update(GetGyroYaw(), GetModulePositions()); } -void SwerveDrive::Drive(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega) { - frc::ChassisSpeeds newChassisSpeeds = - frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); +void SwerveDrive::Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega) { + wpi::math::ChassisSpeeds newChassisSpeeds = + wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, + GetHeading()); SetChassisSpeeds(newChassisSpeeds, true, false); } -void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, - bool openLoop, bool steerInPlace) { +void SwerveDrive::SetChassisSpeeds( + const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop, + bool steerInPlace) { SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, steerInPlace); this->targetChassisSpeeds = newChassisSpeeds; } void SwerveDrive::SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace) { - std::array desaturatedStates = desiredStates; - frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( - static_cast*>(&desaturatedStates), + const std::array& desiredStates, + bool openLoop, bool steerInPlace) { + std::array desaturatedStates = desiredStates; + wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>( + &desaturatedStates), constants::Swerve::kMaxLinearSpeed); for (int i = 0; i < swerveMods.size(); i++) { swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); @@ -76,7 +80,7 @@ void SwerveDrive::SetModuleStates( void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } -void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { +void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) { if (resetSimPose) { swerveDriveSim.Reset(pose, false); for (int i = 0; i < swerveMods.size(); i++) { @@ -89,20 +93,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); } -frc::Pose2d SwerveDrive::GetPose() const { +wpi::math::Pose2d SwerveDrive::GetPose() const { return poseEstimator.GetEstimatedPosition(); } -frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } +wpi::math::Rotation2d SwerveDrive::GetHeading() const { + return GetPose().Rotation(); +} -frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } +wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const { + return gyro.GetRotation2d(); +} -frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { +wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { return kinematics.ToChassisSpeeds(GetModuleStates()); } -std::array SwerveDrive::GetModuleStates() const { - std::array moduleStates; +std::array SwerveDrive::GetModuleStates() + const { + std::array moduleStates; moduleStates[0] = swerveMods[0].GetState(); moduleStates[1] = swerveMods[1].GetState(); moduleStates[2] = swerveMods[2].GetState(); @@ -110,9 +119,9 @@ std::array SwerveDrive::GetModuleStates() const { return moduleStates; } -std::array SwerveDrive::GetModulePositions() +std::array SwerveDrive::GetModulePositions() const { - std::array modulePositions; + std::array modulePositions; modulePositions[0] = swerveMods[0].GetPosition(); modulePositions[1] = swerveMods[1].GetPosition(); modulePositions[2] = swerveMods[2].GetPosition(); @@ -120,11 +129,11 @@ std::array SwerveDrive::GetModulePositions() return modulePositions; } -std::array SwerveDrive::GetModulePoses() const { - std::array modulePoses; +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; for (int i = 0; i < swerveMods.size(); i++) { const SwerveModule& module = swerveMods[i]; - modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{ module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); } return modulePoses; @@ -132,24 +141,24 @@ std::array SwerveDrive::GetModulePoses() const { void SwerveDrive::Log() { std::string table = "Drive/"; - frc::Pose2d pose = GetPose(); - frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); - frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); - frc::SmartDashboard::PutNumber(table + "Heading", + wpi::math::Pose2d pose = GetPose(); + wpi::SmartDashboard::PutNumber(table + "X", pose.X().to()); + wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + wpi::SmartDashboard::PutNumber(table + "Heading", pose.Rotation().Degrees().to()); - frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); - frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); - frc::SmartDashboard::PutNumber(table + "Target VX", + chassisSpeeds.omega.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "Target VY", + wpi::SmartDashboard::PutNumber(table + "Target VY", targetChassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Target Omega Degrees", - targetChassisSpeeds.omega.convert() + targetChassisSpeeds.omega.convert() .to()); for (auto& module : swerveMods) { @@ -158,8 +167,8 @@ void SwerveDrive::Log() { } void SwerveDrive::SimulationPeriodic() { - std::array driveInputs; - std::array steerInputs; + std::array driveInputs; + std::array steerInputs; for (int i = 0; i < swerveMods.size(); i++) { driveInputs[i] = swerveMods[i].GetDriveVoltage(); steerInputs[i] = swerveMods[i].GetSteerVoltage(); @@ -167,26 +176,26 @@ void SwerveDrive::SimulationPeriodic() { swerveDriveSim.SetDriveInputs(driveInputs); swerveDriveSim.SetSteerInputs(steerInputs); - swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod); auto driveStates = swerveDriveSim.GetDriveStates(); auto steerStates = swerveDriveSim.GetSteerStates(); totalCurrentDraw = 0_A; - std::array driveCurrents = + std::array driveCurrents = swerveDriveSim.GetDriveCurrentDraw(); for (const auto& current : driveCurrents) { totalCurrentDraw += current; } - std::array steerCurrents = + std::array steerCurrents = swerveDriveSim.GetSteerCurrentDraw(); for (const auto& current : steerCurrents) { totalCurrentDraw += current; } for (int i = 0; i < swerveMods.size(); i++) { - units::meter_t drivePos{driveStates[i](0, 0)}; - units::meters_per_second_t driveRate{driveStates[i](1, 0)}; - units::radian_t steerPos{steerStates[i](0, 0)}; - units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + wpi::units::meter_t drivePos{driveStates[i](0, 0)}; + wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + wpi::units::radian_t steerPos{steerStates[i](0, 0)}; + wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)}; swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } @@ -194,6 +203,8 @@ void SwerveDrive::SimulationPeriodic() { gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); } -frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } +wpi::math::Pose2d SwerveDrive::GetSimPose() const { + return swerveDriveSim.GetPose(); +} -units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp index 3cfc9a6d45..4c98f0ce20 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -24,8 +24,8 @@ #include "subsystems/SwerveDriveSim.h" -#include -#include +#include +#include template int sgn(T val) { @@ -33,14 +33,14 @@ int sgn(T val) { } SwerveDriveSim::SwerveDriveSim( - const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : SwerveDriveSim( - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -driveFF.GetKv().to() / driveFF.GetKa().to()) .finished(), @@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim( (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), Eigen::Matrix{0.0, 0.0}}, driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius, - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -steerFF.GetKv().to() / steerFF.GetKa().to()) .finished(), @@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), driveMotor(driveMotor), @@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim( kinematics(kinematics) {} void SwerveDriveSim::SetDriveInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < driveInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } void SwerveDriveSim::SetSteerInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < steerInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } @@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs( Eigen::Matrix SwerveDriveSim::CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS) { + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS) { auto Ax = discA * x; double nextStateVel = Ax(1, 0); double inputToStop = nextStateVel / -discB(1, 0); @@ -129,18 +129,18 @@ Eigen::Matrix SwerveDriveSim::CalculateX( return retVal; } -void SwerveDriveSim::Update(units::second_t dt) { +void SwerveDriveSim::Update(wpi::units::second_t dt) { Eigen::Matrix driveDiscA; Eigen::Matrix driveDiscB; - frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, - &driveDiscB); + wpi::math::DiscretizeABAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, + &driveDiscA, &driveDiscB); Eigen::Matrix steerDiscA; Eigen::Matrix steerDiscB; - frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, - &steerDiscB); + wpi::math::DiscretizeABAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, + &steerDiscA, &steerDiscB); - std::array moduleDeltas; + std::array moduleDeltas; for (int i = 0; i < numModules; i++) { double prevDriveStatePos = driveStates[i](0, 0); @@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) { steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], steerInputs[i], steerKs); double currentSteerStatePos = steerStates[i](0, 0); - moduleDeltas[i] = frc::SwerveModulePosition{ - units::meter_t{currentDriveStatePos - prevDriveStatePos}, - frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + moduleDeltas[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos}, + wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}}; } - frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + wpi::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); pose = pose.Exp(twist); omega = twist.dtheta / dt; } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) { this->pose = pose; if (!preserveMotion) { for (int i = 0; i < numModules; i++) { @@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { } } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, @@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; } -frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } +wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; } -std::array +std::array SwerveDriveSim::GetModulePositions() const { - std::array positions; + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return positions; } -std::array -SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, - units::radian_t steerStdDev) { - std::array positions; +std::array +SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev) { + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)} + + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)} + randDist(generator) * driveStdDev, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + - randDist(generator) * steerStdDev}}; + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; } return positions; } -std::array +std::array SwerveDriveSim::GetModuleStates() { - std::array states; + std::array states; for (int i = 0; i < numModules; i++) { - states[i] = frc::SwerveModuleState{ - units::meters_per_second_t{driveStates[i](1, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + states[i] = wpi::math::SwerveModuleState{ + wpi::units::meters_per_second_t{driveStates[i](1, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return states; } @@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } -units::ampere_t SwerveDriveSim::GetCurrentDraw( - const frc::DCMotor& motor, units::radians_per_second_t velocity, - units::volt_t inputVolts, units::volt_t batteryVolts) const { - units::volt_t effVolts = inputVolts - velocity / motor.Kv; +wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( + const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const { + wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv; if (inputVolts >= 0_V) { effVolts = std::clamp(effVolts, 0_V, inputVolts); } else { @@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() +std::array SwerveDriveSim::GetDriveCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / driveWheelRadius.to(); currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], - frc::RobotController::GetBatteryVoltage()); + wpi::RobotController::GetBatteryVoltage()); } return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() +std::array SwerveDriveSim::GetSteerCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; // TODO: If uncommented we get huge current values.. Not sure how to fix // atm. :( currents[i] = 20_A; // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], - // frc::RobotController::GetBatteryVoltage()); + // wpi::RobotController::GetBatteryVoltage()); } return currents; } -units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { - units::ampere_t total{0}; +wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + wpi::units::ampere_t total{0}; for (const auto& val : GetDriveCurrentDraw()) { total += val; } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp index 4b0e27da6d..3bfa786b22 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp @@ -26,17 +26,17 @@ #include -#include -#include -#include +#include +#include +#include SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) : moduleConstants(consts), - driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), - driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA, moduleConstants.driveEncoderB}), - steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), - steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA, moduleConstants.steerEncoderB}), driveEncoderSim(driveEncoder), steerEncoderSim(steerEncoder) { @@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) } void SwerveModule::Periodic() { - units::volt_t steerPID = units::volt_t{ + wpi::units::volt_t steerPID = wpi::units::volt_t{ steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), desiredState.angle.Radians().to())}; steerMotor.SetVoltage(steerPID); - units::volt_t driveFF = + wpi::units::volt_t driveFF = constants::Swerve::kDriveFF.Calculate(desiredState.speed); - units::volt_t drivePID{0}; + wpi::units::volt_t drivePID{0}; if (!openLoop) { - drivePID = units::volt_t{drivePIDController.Calculate( + drivePID = wpi::units::volt_t{drivePIDController.Calculate( driveEncoder.GetRate(), desiredState.speed.to())}; } driveMotor.SetVoltage(driveFF + drivePID); } -void SwerveModule::SetDesiredState(frc::SwerveModuleState newState, +void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, bool shouldBeOpenLoop, bool steerInPlace) { - frc::Rotation2d currentRotation = GetAbsoluteHeading(); + wpi::math::Rotation2d currentRotation = GetAbsoluteHeading(); newState.Optimize(currentRotation); desiredState = newState; } -frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; } -frc::SwerveModuleState SwerveModule::GetState() const { - return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, - GetAbsoluteHeading()}; +wpi::math::SwerveModuleState SwerveModule::GetState() const { + return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; } -frc::SwerveModulePosition SwerveModule::GetPosition() const { - return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, - GetAbsoluteHeading()}; +wpi::math::SwerveModulePosition SwerveModule::GetPosition() const { + return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; } -units::volt_t SwerveModule::GetDriveVoltage() const { - return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::volt_t SwerveModule::GetSteerVoltage() const { - return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::ampere_t SwerveModule::GetDriveCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const { return driveCurrentSim; } -units::ampere_t SwerveModule::GetSteerCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const { return steerCurrentSim; } @@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { } void SwerveModule::Log() { - frc::SwerveModuleState state = GetState(); + wpi::math::SwerveModuleState state = GetState(); std::string table = "Module " + std::to_string(moduleConstants.moduleNum) + "/"; - frc::SmartDashboard::PutNumber(table + "Steer Degrees", - frc::AngleModulus(state.angle.Radians()) - .convert() + wpi::SmartDashboard::PutNumber(table + "Steer Degrees", + wpi::math::AngleModulus(state.angle.Radians()) + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Steer Target Degrees", - units::radian_t{steerPIDController.GetSetpoint()} - .convert() + wpi::units::radian_t{steerPIDController.GetSetpoint()} + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Feet", - state.speed.convert().to()); - frc::SmartDashboard::PutNumber( + state.speed.convert().to()); + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Target Feet", - desiredState.speed.convert().to()); - frc::SmartDashboard::PutNumber(table + "Drive Current", + desiredState.speed.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Drive Current", driveCurrentSim.to()); - frc::SmartDashboard::PutNumber(table + "Steer Current", + wpi::SmartDashboard::PutNumber(table + "Steer Current", steerCurrentSim.to()); } void SwerveModule::SimulationUpdate( - units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent) { + wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); driveEncoderSim.SetRate(driveEncoderRate.to()); driveCurrentSim = driveCurrent; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h index 70a5d74bd5..1e27703dfa 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h @@ -26,25 +26,26 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace constants { namespace Vision { inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; -inline const frc::Transform3d kRobotToCam{ - frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, - frc::Rotation3d{0_rad, -30_deg, 0_rad}}; -inline const frc::AprilTagFieldLayout kTagLayout{ - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)}; +inline const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m}, + wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}}; +inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{ + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::kDefaultField)}; inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; @@ -52,23 +53,23 @@ inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; namespace Swerve { using namespace units; -inline constexpr units::meter_t kTrackWidth{18.5_in}; -inline constexpr units::meter_t kTrackLength{18.5_in}; -inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; -inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; -inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; -inline constexpr units::meter_t kWheelDiameter{4_in}; -inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * +inline constexpr wpi::units::meter_t kTrackWidth{18.5_in}; +inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; +inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; +inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; -inline constexpr units::meter_t kDriveDistPerPulse = +inline constexpr wpi::units::meter_t kDriveDistPerPulse = kWheelCircumference / 1024.0 / kDriveGearRatio; -inline constexpr units::radian_t kSteerRadPerPulse = - units::radian_t{2 * std::numbers::pi} / 1024.0; +inline constexpr wpi::units::radian_t kSteerRadPerPulse = + wpi::units::radian_t{2 * std::numbers::pi} / 1024.0; inline constexpr double kDriveKP = 1.0; inline constexpr double kDriveKI = 0.0; @@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25; using namespace units; -inline const frc::SimpleMotorFeedforward kDriveFF{ +inline const wpi::math::SimpleMotorFeedforward kDriveFF{ 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; -inline const frc::SimpleMotorFeedforward kSteerFF{ +inline const wpi::math::SimpleMotorFeedforward kSteerFF{ 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; struct ModuleConstants { @@ -96,12 +97,12 @@ struct ModuleConstants { const int steerEncoderA; const int steerEncoderB; const double angleOffset; - const frc::Translation2d centerOffset; + const wpi::math::Translation2d centerOffset; ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, units::meter_t xOffset, - units::meter_t yOffset) + int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, + wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), @@ -110,7 +111,7 @@ struct ModuleConstants { steerEncoderA(steerEncoderA), steerEncoderB(steerEncoderB), angleOffset(angleOffset), - centerOffset(frc::Translation2d{xOffset, yOffset}) {} + centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {} }; inline const ModuleConstants FL_CONSTANTS{ diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index defc745c16..9b83e17ac4 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -24,15 +24,15 @@ #pragma once -#include -#include #include +#include +#include #include "Constants.h" #include "VisionSim.h" #include "subsystems/SwerveDrive.h" -class Robot : public frc::TimedRobot { +class Robot : public wpi::TimedRobot { public: void RobotInit() override; void RobotPeriodic() override; @@ -54,6 +54,6 @@ class Robot : public frc::TimedRobot { photon::PhotonCamera camera{constants::Vision::kCameraName}; SwerveDrive drivetrain{}; VisionSim vision{&camera}; - frc::XboxController controller{0}; + wpi::XboxController controller{0}; static constexpr double VISION_TURN_kP = 0.01; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h index 3520165299..fffc5b1d3b 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h @@ -27,28 +27,28 @@ #include #include -#include -#include #include #include #include #include #include #include +#include +#include #include "Constants.h" class VisionSim { public: explicit VisionSim(photon::PhotonCamera* camera) { - if (frc::RobotBase::IsSimulation()) { + if (wpi::RobotBase::IsSimulation()) { visionSim = std::make_unique("main"); visionSim->AddAprilTags(constants::Vision::kTagLayout); cameraProp = std::make_unique(); - cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg}); + cameraProp->SetCalibration(320, 240, wpi::math::Rotation2d{90_deg}); cameraProp->SetCalibError(.35, .10); cameraProp->SetFPS(70_Hz); cameraProp->SetAvgLatency(30_ms); @@ -64,17 +64,17 @@ class VisionSim { photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } - void SimPeriodic(frc::Pose2d robotSimPose) { + void SimPeriodic(wpi::math::Pose2d robotSimPose) { visionSim->Update(robotSimPose); } - void ResetSimPose(frc::Pose2d pose) { - if (frc::RobotBase::IsSimulation()) { + void ResetSimPose(wpi::math::Pose2d pose) { + if (wpi::RobotBase::IsSimulation()) { visionSim->ResetRobotPose(pose); } } - frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } private: std::unique_ptr visionSim; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h index 98b0b2a582..09ebbb0402 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h @@ -26,9 +26,9 @@ #include #include -#include -#include #include +#include +#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -37,26 +37,26 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega); - void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega); + void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); void SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace); + const std::array& desiredStates, + bool openLoop, bool steerInPlace); void Stop(); - void ResetPose(const frc::Pose2d& pose, bool resetSimPose); - frc::Pose2d GetPose() const; - frc::Rotation2d GetHeading() const; - frc::Rotation2d GetGyroYaw() const; - frc::ChassisSpeeds GetChassisSpeeds() const; - std::array GetModuleStates() const; - std::array GetModulePositions() const; - std::array GetModulePoses() const; + void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose); + wpi::math::Pose2d GetPose() const; + wpi::math::Rotation2d GetHeading() const; + wpi::math::Rotation2d GetGyroYaw() const; + wpi::math::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; void Log(); void SimulationPeriodic(); - frc::Pose2d GetSimPose() const; - units::ampere_t GetCurrentDraw() const; + wpi::math::Pose2d GetSimPose() const; + wpi::units::ampere_t GetCurrentDraw() const; private: std::array swerveMods{ @@ -64,17 +64,17 @@ class SwerveDrive { SwerveModule{constants::Swerve::FR_CONSTANTS}, SwerveModule{constants::Swerve::BL_CONSTANTS}, SwerveModule{constants::Swerve::BR_CONSTANTS}}; - frc::SwerveDriveKinematics<4> kinematics{ + wpi::math::SwerveDriveKinematics<4> kinematics{ swerveMods[0].GetModuleConstants().centerOffset, swerveMods[1].GetModuleConstants().centerOffset, swerveMods[2].GetModuleConstants().centerOffset, swerveMods[3].GetModuleConstants().centerOffset, }; - frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; - frc::SwerveDrivePoseEstimator<4> poseEstimator; - frc::ChassisSpeeds targetChassisSpeeds{}; + wpi::ADXRS450_Gyro gyro{wpi::SPI::Port::kOnboardCS0}; + wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; + wpi::math::ChassisSpeeds targetChassisSpeeds{}; - frc::sim::ADXRS450_GyroSim gyroSim; + wpi::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; - units::ampere_t totalCurrentDraw{0}; + wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h index c1cee3e6f1..5ace0164dd 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h @@ -26,77 +26,80 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include static constexpr int numModules{4}; class SwerveDriveSim { public: - SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, - units::volt_t driveKs, const frc::DCMotor& driveMotor, - double driveGearing, units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, - units::volt_t steerKs, const frc::DCMotor& steerMotor, - double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - void SetDriveInputs(const std::array& inputs); - void SetSteerInputs(const std::array& inputs); + SwerveDriveSim( + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + SwerveDriveSim( + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); static Eigen::Matrix CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS); - void Update(units::second_t dt); - void Reset(const frc::Pose2d& pose, bool preserveMotion); - void Reset(const frc::Pose2d& pose, + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS); + void Update(wpi::units::second_t dt); + void Reset(const wpi::math::Pose2d& pose, bool preserveMotion); + void Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, numModules>& moduleSteerStates); - frc::Pose2d GetPose() const; - std::array GetModulePositions() const; - std::array GetNoisyModulePositions( - units::meter_t driveStdDev, units::radian_t steerStdDev); - std::array GetModuleStates(); + wpi::math::Pose2d GetPose() const; + std::array GetModulePositions() + const; + std::array + GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev); + std::array GetModuleStates(); std::array, numModules> GetDriveStates() const; std::array, numModules> GetSteerStates() const; - units::radians_per_second_t GetOmega() const; - units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, - units::radians_per_second_t velocity, - units::volt_t inputVolts, - units::volt_t batteryVolts) const; - std::array GetDriveCurrentDraw() const; - std::array GetSteerCurrentDraw() const; - units::ampere_t GetTotalCurrentDraw() const; + wpi::units::radians_per_second_t GetOmega() const; + wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + wpi::units::ampere_t GetTotalCurrentDraw() const; private: std::random_device rd{}; std::mt19937 generator{rd()}; std::normal_distribution randDist{0.0, 1.0}; - const frc::LinearSystem<2, 1, 2> drivePlant; - const units::volt_t driveKs; - const frc::DCMotor driveMotor; + const wpi::math::LinearSystem<2, 1, 2> drivePlant; + const wpi::units::volt_t driveKs; + const wpi::math::DCMotor driveMotor; const double driveGearing; - const units::meter_t driveWheelRadius; - const frc::LinearSystem<2, 1, 2> steerPlant; - const units::volt_t steerKs; - const frc::DCMotor steerMotor; + const wpi::units::meter_t driveWheelRadius; + const wpi::math::LinearSystem<2, 1, 2> steerPlant; + const wpi::units::volt_t steerKs; + const wpi::math::DCMotor steerMotor; const double steerGearing; - const frc::SwerveDriveKinematics kinematics; - std::array driveInputs{}; + const wpi::math::SwerveDriveKinematics kinematics; + std::array driveInputs{}; std::array, numModules> driveStates{}; - std::array steerInputs{}; + std::array steerInputs{}; std::array, numModules> steerStates{}; - frc::Pose2d pose{frc::Pose2d{}}; - units::radians_per_second_t omega{0}; + wpi::math::Pose2d pose{wpi::math::Pose2d{}}; + wpi::units::radians_per_second_t omega{0}; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h index 9c07ab426d..cb9a9b2df4 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h @@ -24,13 +24,13 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "Constants.h" @@ -38,44 +38,44 @@ class SwerveModule { public: explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); void Periodic(); - void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop, - bool steerInPlace); - frc::Rotation2d GetAbsoluteHeading() const; - frc::SwerveModuleState GetState() const; - frc::SwerveModulePosition GetPosition() const; - units::volt_t GetDriveVoltage() const; - units::volt_t GetSteerVoltage() const; - units::ampere_t GetDriveCurrentSim() const; - units::ampere_t GetSteerCurrentSim() const; + void SetDesiredState(wpi::math::SwerveModuleState newState, + bool shouldBeOpenLoop, bool steerInPlace); + wpi::math::Rotation2d GetAbsoluteHeading() const; + wpi::math::SwerveModuleState GetState() const; + wpi::math::SwerveModulePosition GetPosition() const; + wpi::units::volt_t GetDriveVoltage() const; + wpi::units::volt_t GetSteerVoltage() const; + wpi::units::ampere_t GetDriveCurrentSim() const; + wpi::units::ampere_t GetSteerCurrentSim() const; constants::Swerve::ModuleConstants GetModuleConstants() const; void Log(); - void SimulationUpdate(units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, - units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent); + void SimulationUpdate(wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent); private: const constants::Swerve::ModuleConstants moduleConstants; - frc::PWMSparkMax driveMotor; - frc::Encoder driveEncoder; - frc::PWMSparkMax steerMotor; - frc::Encoder steerEncoder; + wpi::PWMSparkMax driveMotor; + wpi::Encoder driveEncoder; + wpi::PWMSparkMax steerMotor; + wpi::Encoder steerEncoder; - frc::SwerveModuleState desiredState{}; + wpi::math::SwerveModuleState desiredState{}; bool openLoop{false}; - frc::PIDController drivePIDController{constants::Swerve::kDriveKP, - constants::Swerve::kDriveKI, - constants::Swerve::kDriveKD}; - frc::PIDController steerPIDController{constants::Swerve::kSteerKP, - constants::Swerve::kSteerKI, - constants::Swerve::kSteerKD}; + wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; - frc::sim::EncoderSim driveEncoderSim; - units::ampere_t driveCurrentSim{0}; - frc::sim::EncoderSim steerEncoderSim; - units::ampere_t steerCurrentSim{0}; + wpi::sim::EncoderSim driveEncoderSim; + wpi::units::ampere_t driveCurrentSim{0}; + wpi::sim::EncoderSim steerEncoderSim; + wpi::units::ampere_t steerCurrentSim{0}; }; diff --git a/photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp b/photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp index 031d1ce96b..4fcf3fefd8 100644 --- a/photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp +++ b/photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include +#include #include "gtest/gtest.h" diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 92aaaed289..4bf65dc3b1 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp @@ -24,8 +24,8 @@ #include "Robot.h" -#include -#include +#include +#include void Robot::RobotInit() {} @@ -50,7 +50,7 @@ void Robot::AutonomousPeriodic() {} void Robot::AutonomousExit() {} void Robot::TeleopInit() { - frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + wpi::math::Pose2d pose{1_m, 1_m, wpi::math::Rotation2d{}}; drivetrain.ResetPose(pose, true); } @@ -68,7 +68,7 @@ void Robot::TeleopPeriodic() { // Calculate whether the gamepiece launcher runs based on our global pose // estimate. - frc::Pose2d curPose = drivetrain.GetPose(); + wpi::math::Pose2d curPose = drivetrain.GetPose(); bool shouldRun = (curPose.Y() > 2.0_m && curPose.X() < 4.0_m); // Close enough to blue speaker launcher.setRunning(shouldRun); @@ -87,20 +87,20 @@ void Robot::SimulationPeriodic() { drivetrain.SimulationPeriodic(); vision.SimPeriodic(drivetrain.GetSimPose()); - frc::Field2d& debugField = vision.GetSimDebugField(); + wpi::Field2d& debugField = vision.GetSimDebugField(); debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); debugField.GetObject("EstimatedRobotModules") ->SetPoses(drivetrain.GetModulePoses()); - units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); - units::volt_t loadedBattVolts = - frc::sim::BatterySim::Calculate({totalCurrent}); + wpi::units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + wpi::units::volt_t loadedBattVolts = + wpi::sim::BatterySim::Calculate({totalCurrent}); // Using max(0.1, voltage) here isn't a *physically correct* solution, // but it avoids problems with battery voltage measuring 0. - frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); + wpi::sim::RoboRioSim::SetVInVoltage(wpi::units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } +int main() { return wpi::StartRobot(); } #endif diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp index fc61a731ff..c2a04955b7 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp @@ -25,7 +25,7 @@ #include "subsystems/GamepieceLauncher.h" // Include the header file GamepieceLauncher::GamepieceLauncher() { - motor = new frc::PWMSparkMax(8); // Create the motor object for PWM port 8 + motor = new wpi::PWMSparkMax(8); // Create the motor object for PWM port 8 simulationInit(); } @@ -36,23 +36,23 @@ void GamepieceLauncher::setRunning(bool shouldRun) { void GamepieceLauncher::periodic() { // Calculate the maximum RPM double maxRPM = - units::radians_per_second_t(frc::DCMotor::Falcon500(1).freeSpeed) + wpi::units::radians_per_second_t(wpi::math::DCMotor::Falcon500(1).freeSpeed) .to() * 60 / (2 * std::numbers::pi); curMotorCmd = curDesSpd / maxRPM; curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0); motor->Set(curMotorCmd); - frc::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd); + wpi::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd); } void GamepieceLauncher::simulationPeriodic() { launcherSim.SetInputVoltage(curMotorCmd * - frc::RobotController::GetBatteryVoltage()); + wpi::RobotController::GetBatteryVoltage()); launcherSim.Update(0.02_s); auto spd = launcherSim.GetAngularVelocity().to() * 60 / (2 * std::numbers::pi); - frc::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd); + wpi::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd); } void GamepieceLauncher::simulationInit() {} diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp index 38b2387a77..19fe852160 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp @@ -26,18 +26,19 @@ #include -#include -#include +#include +#include SwerveDrive::SwerveDrive() : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), - frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), gyroSim(gyro), - swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kDriveGearRatio, - constants::Swerve::kWheelDiameter / 2, - constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), - constants::Swerve::kSteerGearRatio, kinematics) {} + swerveDriveSim( + constants::Swerve::kDriveFF, wpi::math::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, constants::Swerve::kSteerFF, + wpi::math::DCMotor::Falcon500(1), constants::Swerve::kSteerGearRatio, + kinematics) {} void SwerveDrive::Periodic() { for (auto& currentModule : swerveMods) { @@ -47,27 +48,30 @@ void SwerveDrive::Periodic() { poseEstimator.Update(GetGyroYaw(), GetModulePositions()); } -void SwerveDrive::Drive(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega) { - frc::ChassisSpeeds newChassisSpeeds = - frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); +void SwerveDrive::Drive(wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega) { + wpi::math::ChassisSpeeds newChassisSpeeds = + wpi::math::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, + GetHeading()); SetChassisSpeeds(newChassisSpeeds, true, false); } -void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, - bool openLoop, bool steerInPlace) { +void SwerveDrive::SetChassisSpeeds( + const wpi::math::ChassisSpeeds& newChassisSpeeds, bool openLoop, + bool steerInPlace) { SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, steerInPlace); this->targetChassisSpeeds = newChassisSpeeds; } void SwerveDrive::SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace) { - std::array desaturatedStates = desiredStates; - frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( - static_cast*>(&desaturatedStates), + const std::array& desiredStates, + bool openLoop, bool steerInPlace) { + std::array desaturatedStates = desiredStates; + wpi::math::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>( + &desaturatedStates), constants::Swerve::kMaxLinearSpeed); for (int i = 0; i < swerveMods.size(); i++) { swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); @@ -76,19 +80,19 @@ void SwerveDrive::SetModuleStates( void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } -void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, - units::second_t timestamp) { +void SwerveDrive::AddVisionMeasurement( + const wpi::math::Pose2d& visionMeasurement, wpi::units::second_t timestamp) { poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); } -void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, - units::second_t timestamp, - const Eigen::Vector3d& stdDevs) { - wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; +void SwerveDrive::AddVisionMeasurement( + const wpi::math::Pose2d& visionMeasurement, wpi::units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + wpi::util::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); } -void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { +void SwerveDrive::ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose) { if (resetSimPose) { swerveDriveSim.Reset(pose, false); for (int i = 0; i < swerveMods.size(); i++) { @@ -101,20 +105,25 @@ void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); } -frc::Pose2d SwerveDrive::GetPose() const { +wpi::math::Pose2d SwerveDrive::GetPose() const { return poseEstimator.GetEstimatedPosition(); } -frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } +wpi::math::Rotation2d SwerveDrive::GetHeading() const { + return GetPose().Rotation(); +} -frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } +wpi::math::Rotation2d SwerveDrive::GetGyroYaw() const { + return gyro.GetRotation2d(); +} -frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { +wpi::math::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { return kinematics.ToChassisSpeeds(GetModuleStates()); } -std::array SwerveDrive::GetModuleStates() const { - std::array moduleStates; +std::array SwerveDrive::GetModuleStates() + const { + std::array moduleStates; moduleStates[0] = swerveMods[0].GetState(); moduleStates[1] = swerveMods[1].GetState(); moduleStates[2] = swerveMods[2].GetState(); @@ -122,9 +131,9 @@ std::array SwerveDrive::GetModuleStates() const { return moduleStates; } -std::array SwerveDrive::GetModulePositions() +std::array SwerveDrive::GetModulePositions() const { - std::array modulePositions; + std::array modulePositions; modulePositions[0] = swerveMods[0].GetPosition(); modulePositions[1] = swerveMods[1].GetPosition(); modulePositions[2] = swerveMods[2].GetPosition(); @@ -132,11 +141,11 @@ std::array SwerveDrive::GetModulePositions() return modulePositions; } -std::array SwerveDrive::GetModulePoses() const { - std::array modulePoses; +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; for (int i = 0; i < swerveMods.size(); i++) { const SwerveModule& module = swerveMods[i]; - modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + modulePoses[i] = GetPose().TransformBy(wpi::math::Transform2d{ module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); } return modulePoses; @@ -144,24 +153,24 @@ std::array SwerveDrive::GetModulePoses() const { void SwerveDrive::Log() { std::string table = "Drive/"; - frc::Pose2d pose = GetPose(); - frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); - frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); - frc::SmartDashboard::PutNumber(table + "Heading", + wpi::math::Pose2d pose = GetPose(); + wpi::SmartDashboard::PutNumber(table + "X", pose.X().to()); + wpi::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + wpi::SmartDashboard::PutNumber(table + "Heading", pose.Rotation().Degrees().to()); - frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); - frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::math::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + wpi::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + wpi::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + wpi::SmartDashboard::PutNumber( table + "Omega Degrees", - chassisSpeeds.omega.convert().to()); - frc::SmartDashboard::PutNumber(table + "Target VX", + chassisSpeeds.omega.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Target VX", targetChassisSpeeds.vx.to()); - frc::SmartDashboard::PutNumber(table + "Target VY", + wpi::SmartDashboard::PutNumber(table + "Target VY", targetChassisSpeeds.vy.to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Target Omega Degrees", - targetChassisSpeeds.omega.convert() + targetChassisSpeeds.omega.convert() .to()); for (auto& module : swerveMods) { @@ -170,8 +179,8 @@ void SwerveDrive::Log() { } void SwerveDrive::SimulationPeriodic() { - std::array driveInputs; - std::array steerInputs; + std::array driveInputs; + std::array steerInputs; for (int i = 0; i < swerveMods.size(); i++) { driveInputs[i] = swerveMods[i].GetDriveVoltage(); steerInputs[i] = swerveMods[i].GetSteerVoltage(); @@ -179,26 +188,26 @@ void SwerveDrive::SimulationPeriodic() { swerveDriveSim.SetDriveInputs(driveInputs); swerveDriveSim.SetSteerInputs(steerInputs); - swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + swerveDriveSim.Update(wpi::TimedRobot::kDefaultPeriod); auto driveStates = swerveDriveSim.GetDriveStates(); auto steerStates = swerveDriveSim.GetSteerStates(); totalCurrentDraw = 0_A; - std::array driveCurrents = + std::array driveCurrents = swerveDriveSim.GetDriveCurrentDraw(); for (const auto& current : driveCurrents) { totalCurrentDraw += current; } - std::array steerCurrents = + std::array steerCurrents = swerveDriveSim.GetSteerCurrentDraw(); for (const auto& current : steerCurrents) { totalCurrentDraw += current; } for (int i = 0; i < swerveMods.size(); i++) { - units::meter_t drivePos{driveStates[i](0, 0)}; - units::meters_per_second_t driveRate{driveStates[i](1, 0)}; - units::radian_t steerPos{steerStates[i](0, 0)}; - units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + wpi::units::meter_t drivePos{driveStates[i](0, 0)}; + wpi::units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + wpi::units::radian_t steerPos{steerStates[i](0, 0)}; + wpi::units::radians_per_second_t steerRate{steerStates[i](1, 0)}; swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); } @@ -206,6 +215,8 @@ void SwerveDrive::SimulationPeriodic() { gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); } -frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } +wpi::math::Pose2d SwerveDrive::GetSimPose() const { + return swerveDriveSim.GetPose(); +} -units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } +wpi::units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp index 3cfc9a6d45..9f3713c888 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -24,8 +24,8 @@ #include "subsystems/SwerveDriveSim.h" -#include -#include +#include +#include template int sgn(T val) { @@ -33,14 +33,14 @@ int sgn(T val) { } SwerveDriveSim::SwerveDriveSim( - const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : SwerveDriveSim( - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -driveFF.GetKv().to() / driveFF.GetKa().to()) .finished(), @@ -49,7 +49,7 @@ SwerveDriveSim::SwerveDriveSim( (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), Eigen::Matrix{0.0, 0.0}}, driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius, - frc::LinearSystem<2, 1, 2>{ + wpi::math::LinearSystem<2, 1, 2>{ (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, -steerFF.GetKv().to() / steerFF.GetKa().to()) .finished(), @@ -60,12 +60,12 @@ SwerveDriveSim::SwerveDriveSim( steerFF.GetKs(), steerMotor, steerGearing, kinematics) {} SwerveDriveSim::SwerveDriveSim( - const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics) + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics) : drivePlant(drivePlant), driveKs(driveKs), driveMotor(driveMotor), @@ -78,19 +78,19 @@ SwerveDriveSim::SwerveDriveSim( kinematics(kinematics) {} void SwerveDriveSim::SetDriveInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < driveInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } void SwerveDriveSim::SetSteerInputs( - const std::array& inputs) { - units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + const std::array& inputs) { + wpi::units::volt_t battVoltage = wpi::RobotController::GetBatteryVoltage(); for (int i = 0; i < steerInputs.size(); i++) { - units::volt_t input = inputs[i]; + wpi::units::volt_t input = inputs[i]; steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); } } @@ -98,8 +98,8 @@ void SwerveDriveSim::SetSteerInputs( Eigen::Matrix SwerveDriveSim::CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS) { + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS) { auto Ax = discA * x; double nextStateVel = Ax(1, 0); double inputToStop = nextStateVel / -discB(1, 0); @@ -129,18 +129,18 @@ Eigen::Matrix SwerveDriveSim::CalculateX( return retVal; } -void SwerveDriveSim::Update(units::second_t dt) { +void SwerveDriveSim::Update(wpi::units::second_t dt) { Eigen::Matrix driveDiscA; Eigen::Matrix driveDiscB; - frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, - &driveDiscB); + wpi::math::DiscretizeABAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, + &driveDiscA, &driveDiscB); Eigen::Matrix steerDiscA; Eigen::Matrix steerDiscB; - frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, - &steerDiscB); + wpi::math::DiscretizeABAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, + &steerDiscA, &steerDiscB); - std::array moduleDeltas; + std::array moduleDeltas; for (int i = 0; i < numModules; i++) { double prevDriveStatePos = driveStates[i](0, 0); @@ -150,17 +150,17 @@ void SwerveDriveSim::Update(units::second_t dt) { steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], steerInputs[i], steerKs); double currentSteerStatePos = steerStates[i](0, 0); - moduleDeltas[i] = frc::SwerveModulePosition{ - units::meter_t{currentDriveStatePos - prevDriveStatePos}, - frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + moduleDeltas[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{currentDriveStatePos - prevDriveStatePos}, + wpi::math::Rotation2d{wpi::units::radian_t{currentSteerStatePos}}}; } - frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + wpi::math::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); pose = pose.Exp(twist); omega = twist.dtheta / dt; } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, bool preserveMotion) { this->pose = pose; if (!preserveMotion) { for (int i = 0; i < numModules; i++) { @@ -171,7 +171,7 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { } } -void SwerveDriveSim::Reset(const frc::Pose2d& pose, +void SwerveDriveSim::Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, @@ -182,40 +182,40 @@ void SwerveDriveSim::Reset(const frc::Pose2d& pose, omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; } -frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } +wpi::math::Pose2d SwerveDriveSim::GetPose() const { return pose; } -std::array +std::array SwerveDriveSim::GetModulePositions() const { - std::array positions; + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return positions; } -std::array -SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, - units::radian_t steerStdDev) { - std::array positions; +std::array +SwerveDriveSim::GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev) { + std::array positions; for (int i = 0; i < numModules; i++) { - positions[i] = frc::SwerveModulePosition{ - units::meter_t{driveStates[i](0, 0)} + + positions[i] = wpi::math::SwerveModulePosition{ + wpi::units::meter_t{driveStates[i](0, 0)} + randDist(generator) * driveStdDev, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + - randDist(generator) * steerStdDev}}; + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; } return positions; } -std::array +std::array SwerveDriveSim::GetModuleStates() { - std::array states; + std::array states; for (int i = 0; i < numModules; i++) { - states[i] = frc::SwerveModuleState{ - units::meters_per_second_t{driveStates[i](1, 0)}, - frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + states[i] = wpi::math::SwerveModuleState{ + wpi::units::meters_per_second_t{driveStates[i](1, 0)}, + wpi::math::Rotation2d{wpi::units::radian_t{steerStates[i](0, 0)}}}; } return states; } @@ -230,12 +230,12 @@ SwerveDriveSim::GetSteerStates() const { return steerStates; } -units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } +wpi::units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } -units::ampere_t SwerveDriveSim::GetCurrentDraw( - const frc::DCMotor& motor, units::radians_per_second_t velocity, - units::volt_t inputVolts, units::volt_t batteryVolts) const { - units::volt_t effVolts = inputVolts - velocity / motor.Kv; +wpi::units::ampere_t SwerveDriveSim::GetCurrentDraw( + const wpi::math::DCMotor& motor, wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, wpi::units::volt_t batteryVolts) const { + wpi::units::volt_t effVolts = inputVolts - velocity / motor.Kv; if (inputVolts >= 0_V) { effVolts = std::clamp(effVolts, 0_V, inputVolts); } else { @@ -245,36 +245,36 @@ units::ampere_t SwerveDriveSim::GetCurrentDraw( return retVal; } -std::array SwerveDriveSim::GetDriveCurrentDraw() +std::array SwerveDriveSim::GetDriveCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / driveWheelRadius.to(); currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], - frc::RobotController::GetBatteryVoltage()); + wpi::RobotController::GetBatteryVoltage()); } return currents; } -std::array SwerveDriveSim::GetSteerCurrentDraw() +std::array SwerveDriveSim::GetSteerCurrentDraw() const { - std::array currents; + std::array currents; for (int i = 0; i < numModules; i++) { - units::radians_per_second_t speed = - units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + wpi::units::radians_per_second_t speed = + wpi::units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; // TODO: If uncommented we get huge current values.. Not sure how to fix // atm. :( currents[i] = 20_A; // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], - // frc::RobotController::GetBatteryVoltage()); + // wpi::RobotController::GetBatteryVoltage()); } return currents; } -units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { - units::ampere_t total{0}; +wpi::units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + wpi::units::ampere_t total{0}; for (const auto& val : GetDriveCurrentDraw()) { total += val; } diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp index 4b0e27da6d..3bfa786b22 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp @@ -26,17 +26,17 @@ #include -#include -#include -#include +#include +#include +#include SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) : moduleConstants(consts), - driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), - driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + driveMotor(wpi::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(wpi::Encoder{moduleConstants.driveEncoderA, moduleConstants.driveEncoderB}), - steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), - steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + steerMotor(wpi::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(wpi::Encoder{moduleConstants.steerEncoderA, moduleConstants.steerEncoderB}), driveEncoderSim(driveEncoder), steerEncoderSim(steerEncoder) { @@ -48,55 +48,55 @@ SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) } void SwerveModule::Periodic() { - units::volt_t steerPID = units::volt_t{ + wpi::units::volt_t steerPID = wpi::units::volt_t{ steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), desiredState.angle.Radians().to())}; steerMotor.SetVoltage(steerPID); - units::volt_t driveFF = + wpi::units::volt_t driveFF = constants::Swerve::kDriveFF.Calculate(desiredState.speed); - units::volt_t drivePID{0}; + wpi::units::volt_t drivePID{0}; if (!openLoop) { - drivePID = units::volt_t{drivePIDController.Calculate( + drivePID = wpi::units::volt_t{drivePIDController.Calculate( driveEncoder.GetRate(), desiredState.speed.to())}; } driveMotor.SetVoltage(driveFF + drivePID); } -void SwerveModule::SetDesiredState(frc::SwerveModuleState newState, +void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState newState, bool shouldBeOpenLoop, bool steerInPlace) { - frc::Rotation2d currentRotation = GetAbsoluteHeading(); + wpi::math::Rotation2d currentRotation = GetAbsoluteHeading(); newState.Optimize(currentRotation); desiredState = newState; } -frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { - return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +wpi::math::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return wpi::math::Rotation2d{wpi::units::radian_t{steerEncoder.GetDistance()}}; } -frc::SwerveModuleState SwerveModule::GetState() const { - return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, - GetAbsoluteHeading()}; +wpi::math::SwerveModuleState SwerveModule::GetState() const { + return wpi::math::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; } -frc::SwerveModulePosition SwerveModule::GetPosition() const { - return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, - GetAbsoluteHeading()}; +wpi::math::SwerveModulePosition SwerveModule::GetPosition() const { + return wpi::math::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; } -units::volt_t SwerveModule::GetDriveVoltage() const { - return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::volt_t SwerveModule::GetSteerVoltage() const { - return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +wpi::units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * wpi::RobotController::GetBatteryVoltage(); } -units::ampere_t SwerveModule::GetDriveCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetDriveCurrentSim() const { return driveCurrentSim; } -units::ampere_t SwerveModule::GetSteerCurrentSim() const { +wpi::units::ampere_t SwerveModule::GetSteerCurrentSim() const { return steerCurrentSim; } @@ -105,37 +105,37 @@ constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { } void SwerveModule::Log() { - frc::SwerveModuleState state = GetState(); + wpi::math::SwerveModuleState state = GetState(); std::string table = "Module " + std::to_string(moduleConstants.moduleNum) + "/"; - frc::SmartDashboard::PutNumber(table + "Steer Degrees", - frc::AngleModulus(state.angle.Radians()) - .convert() + wpi::SmartDashboard::PutNumber(table + "Steer Degrees", + wpi::math::AngleModulus(state.angle.Radians()) + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Steer Target Degrees", - units::radian_t{steerPIDController.GetSetpoint()} - .convert() + wpi::units::radian_t{steerPIDController.GetSetpoint()} + .convert() .to()); - frc::SmartDashboard::PutNumber( + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Feet", - state.speed.convert().to()); - frc::SmartDashboard::PutNumber( + state.speed.convert().to()); + wpi::SmartDashboard::PutNumber( table + "Drive Velocity Target Feet", - desiredState.speed.convert().to()); - frc::SmartDashboard::PutNumber(table + "Drive Current", + desiredState.speed.convert().to()); + wpi::SmartDashboard::PutNumber(table + "Drive Current", driveCurrentSim.to()); - frc::SmartDashboard::PutNumber(table + "Steer Current", + wpi::SmartDashboard::PutNumber(table + "Steer Current", steerCurrentSim.to()); } void SwerveModule::SimulationUpdate( - units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent) { + wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent) { driveEncoderSim.SetDistance(driveEncoderDist.to()); driveEncoderSim.SetRate(driveEncoderRate.to()); driveCurrentSim = driveCurrent; diff --git a/photonlib-cpp-examples/poseest/src/main/include/Constants.h b/photonlib-cpp-examples/poseest/src/main/include/Constants.h index 70a5d74bd5..1e27703dfa 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Constants.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Constants.h @@ -26,25 +26,26 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace constants { namespace Vision { inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; -inline const frc::Transform3d kRobotToCam{ - frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, - frc::Rotation3d{0_rad, -30_deg, 0_rad}}; -inline const frc::AprilTagFieldLayout kTagLayout{ - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::kDefaultField)}; +inline const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m}, + wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}}; +inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{ + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::kDefaultField)}; inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; @@ -52,23 +53,23 @@ inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; namespace Swerve { using namespace units; -inline constexpr units::meter_t kTrackWidth{18.5_in}; -inline constexpr units::meter_t kTrackLength{18.5_in}; -inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; -inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; -inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; -inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; -inline constexpr units::meter_t kWheelDiameter{4_in}; -inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * +inline constexpr wpi::units::meter_t kTrackWidth{18.5_in}; +inline constexpr wpi::units::meter_t kTrackLength{18.5_in}; +inline constexpr wpi::units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr wpi::units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr wpi::units::meter_t kWheelDiameter{4_in}; +inline constexpr wpi::units::meter_t kWheelCircumference{kWheelDiameter * std::numbers::pi}; inline constexpr double kDriveGearRatio = 6.75; inline constexpr double kSteerGearRatio = 12.8; -inline constexpr units::meter_t kDriveDistPerPulse = +inline constexpr wpi::units::meter_t kDriveDistPerPulse = kWheelCircumference / 1024.0 / kDriveGearRatio; -inline constexpr units::radian_t kSteerRadPerPulse = - units::radian_t{2 * std::numbers::pi} / 1024.0; +inline constexpr wpi::units::radian_t kSteerRadPerPulse = + wpi::units::radian_t{2 * std::numbers::pi} / 1024.0; inline constexpr double kDriveKP = 1.0; inline constexpr double kDriveKI = 0.0; @@ -80,10 +81,10 @@ inline constexpr double kSteerKD = 0.25; using namespace units; -inline const frc::SimpleMotorFeedforward kDriveFF{ +inline const wpi::math::SimpleMotorFeedforward kDriveFF{ 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; -inline const frc::SimpleMotorFeedforward kSteerFF{ +inline const wpi::math::SimpleMotorFeedforward kSteerFF{ 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; struct ModuleConstants { @@ -96,12 +97,12 @@ struct ModuleConstants { const int steerEncoderA; const int steerEncoderB; const double angleOffset; - const frc::Translation2d centerOffset; + const wpi::math::Translation2d centerOffset; ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, int driveEncoderB, int steerMotorId, int steerEncoderA, - int steerEncoderB, double angleOffset, units::meter_t xOffset, - units::meter_t yOffset) + int steerEncoderB, double angleOffset, wpi::units::meter_t xOffset, + wpi::units::meter_t yOffset) : moduleNum(moduleNum), driveMotorId(driveMotorId), driveEncoderA(driveEncoderA), @@ -110,7 +111,7 @@ struct ModuleConstants { steerEncoderA(steerEncoderA), steerEncoderB(steerEncoderB), angleOffset(angleOffset), - centerOffset(frc::Translation2d{xOffset, yOffset}) {} + centerOffset(wpi::math::Translation2d{xOffset, yOffset}) {} }; inline const ModuleConstants FL_CONSTANTS{ diff --git a/photonlib-cpp-examples/poseest/src/main/include/Robot.h b/photonlib-cpp-examples/poseest/src/main/include/Robot.h index de874ce73e..284f920325 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Robot.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Robot.h @@ -24,14 +24,14 @@ #pragma once -#include -#include +#include +#include #include "Vision.h" #include "subsystems/GamepieceLauncher.h" #include "subsystems/SwerveDrive.h" -class Robot : public frc::TimedRobot { +class Robot : public wpi::TimedRobot { public: void RobotInit() override; void RobotPeriodic() override; @@ -51,10 +51,10 @@ class Robot : public frc::TimedRobot { private: SwerveDrive drivetrain{}; - Vision vision{[=, this](frc::Pose2d pose, units::second_t timestamp, + Vision vision{[=, this](wpi::math::Pose2d pose, wpi::units::second_t timestamp, Eigen::Matrix stddevs) { drivetrain.AddVisionMeasurement(pose, timestamp, stddevs); }}; GamepieceLauncher launcher{}; - frc::XboxController controller{0}; + wpi::XboxController controller{0}; }; diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h index b3dd50589d..ba2fa80803 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Vision.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h @@ -28,14 +28,14 @@ #include #include -#include -#include #include #include #include #include #include #include +#include +#include #include "Constants.h" @@ -45,18 +45,18 @@ class Vision { * @param estConsumer Lamba that will accept a pose estimate and pass it to * your desired SwerveDrivePoseEstimator. */ - Vision(std::function)> estConsumer) : estConsumer{estConsumer} { - if (frc::RobotBase::IsSimulation()) { + if (wpi::RobotBase::IsSimulation()) { visionSim = std::make_unique("main"); visionSim->AddAprilTags(constants::Vision::kTagLayout); cameraProp = std::make_unique(); - cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibration(960, 720, wpi::math::Rotation2d{90_deg}); cameraProp->SetCalibError(.35, .10); cameraProp->SetFPS(15_Hz); cameraProp->SetAvgLatency(50_ms); @@ -83,7 +83,7 @@ class Vision { m_latestResult = result; // In sim only, add our vision estimate to the sim debug field - if (frc::RobotBase::IsSimulation()) { + if (wpi::RobotBase::IsSimulation()) { if (visionEst) { GetSimDebugField() .GetObject("VisionEstimation") @@ -100,12 +100,13 @@ class Vision { } } - Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix GetEstimationStdDevs( + wpi::math::Pose2d estimatedPose) { Eigen::Matrix estStdDevs = constants::Vision::kSingleTagStdDevs; auto targets = GetLatestResult().GetTargets(); int numTags = 0; - units::meter_t avgDist = 0_m; + wpi::units::meter_t avgDist = 0_m; for (const auto& tgt : targets) { auto tagPose = photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); @@ -133,17 +134,17 @@ class Vision { return estStdDevs; } - void SimPeriodic(frc::Pose2d robotSimPose) { + void SimPeriodic(wpi::math::Pose2d robotSimPose) { visionSim->Update(robotSimPose); } - void ResetSimPose(frc::Pose2d pose) { - if (frc::RobotBase::IsSimulation()) { + void ResetSimPose(wpi::math::Pose2d pose) { + if (wpi::RobotBase::IsSimulation()) { visionSim->ResetRobotPose(pose); } } - frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + wpi::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } private: photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout, @@ -155,6 +156,7 @@ class Vision { // The most recent result, cached for calculating std devs photon::PhotonPipelineResult m_latestResult; - std::function)> + std::function)> estConsumer; }; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h index 41b6d8da45..259449dfe4 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h @@ -27,14 +27,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include class GamepieceLauncher { public: @@ -44,20 +44,21 @@ class GamepieceLauncher { void simulationPeriodic(); // Method to handle simulation updates private: - frc::PWMSparkMax* motor; // Motor controller + wpi::PWMSparkMax* motor; // Motor controller const double LAUNCH_SPEED_RPM = 2500; double curDesSpd = 0.0; double curMotorCmd = 0.0; - static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia = + static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia = 0.5 * 1.5_lb * 4_in * 4_in; - frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(1); - frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem( - m_gearbox, kFlywheelMomentOfInertia, 1.0)}; + wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::Falcon500(1); + wpi::math::LinearSystem<1, 1, 1> m_plant{ + wpi::math::LinearSystemId::FlywheelSystem(m_gearbox, + kFlywheelMomentOfInertia, 1.0)}; - frc::sim::FlywheelSim launcherSim{m_plant, m_gearbox}; + wpi::sim::FlywheelSim launcherSim{m_plant, m_gearbox}; void simulationInit(); // Method to initialize simulation components }; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h index 1fb0bf7e87..9d39b8dda0 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h @@ -26,9 +26,9 @@ #include #include -#include -#include #include +#include +#include #include "SwerveDriveSim.h" #include "SwerveModule.h" @@ -37,31 +37,31 @@ class SwerveDrive { public: SwerveDrive(); void Periodic(); - void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega); - void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + void Drive(wpi::units::meters_per_second_t vx, wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega); + void SetChassisSpeeds(const wpi::math::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); void SetModuleStates( - const std::array& desiredStates, bool openLoop, - bool steerInPlace); + const std::array& desiredStates, + bool openLoop, bool steerInPlace); void Stop(); - void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, - units::second_t timestamp); - void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, - units::second_t timestamp, + void AddVisionMeasurement(const wpi::math::Pose2d& visionMeasurement, + wpi::units::second_t timestamp); + void AddVisionMeasurement(const wpi::math::Pose2d& visionMeasurement, + wpi::units::second_t timestamp, const Eigen::Vector3d& stdDevs); - void ResetPose(const frc::Pose2d& pose, bool resetSimPose); - frc::Pose2d GetPose() const; - frc::Rotation2d GetHeading() const; - frc::Rotation2d GetGyroYaw() const; - frc::ChassisSpeeds GetChassisSpeeds() const; - std::array GetModuleStates() const; - std::array GetModulePositions() const; - std::array GetModulePoses() const; + void ResetPose(const wpi::math::Pose2d& pose, bool resetSimPose); + wpi::math::Pose2d GetPose() const; + wpi::math::Rotation2d GetHeading() const; + wpi::math::Rotation2d GetGyroYaw() const; + wpi::math::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; void Log(); void SimulationPeriodic(); - frc::Pose2d GetSimPose() const; - units::ampere_t GetCurrentDraw() const; + wpi::math::Pose2d GetSimPose() const; + wpi::units::ampere_t GetCurrentDraw() const; private: std::array swerveMods{ @@ -69,17 +69,17 @@ class SwerveDrive { SwerveModule{constants::Swerve::FR_CONSTANTS}, SwerveModule{constants::Swerve::BL_CONSTANTS}, SwerveModule{constants::Swerve::BR_CONSTANTS}}; - frc::SwerveDriveKinematics<4> kinematics{ + wpi::math::SwerveDriveKinematics<4> kinematics{ swerveMods[0].GetModuleConstants().centerOffset, swerveMods[1].GetModuleConstants().centerOffset, swerveMods[2].GetModuleConstants().centerOffset, swerveMods[3].GetModuleConstants().centerOffset, }; - frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; - frc::SwerveDrivePoseEstimator<4> poseEstimator; - frc::ChassisSpeeds targetChassisSpeeds{}; + wpi::ADXRS450_Gyro gyro{wpi::SPI::Port::kOnboardCS0}; + wpi::math::SwerveDrivePoseEstimator<4> poseEstimator; + wpi::math::ChassisSpeeds targetChassisSpeeds{}; - frc::sim::ADXRS450_GyroSim gyroSim; + wpi::sim::ADXRS450_GyroSim gyroSim; SwerveDriveSim swerveDriveSim; - units::ampere_t totalCurrentDraw{0}; + wpi::units::ampere_t totalCurrentDraw{0}; }; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h index c1cee3e6f1..5ace0164dd 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h @@ -26,77 +26,80 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include static constexpr int numModules{4}; class SwerveDriveSim { public: - SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, - const frc::DCMotor& driveMotor, double driveGearing, - units::meter_t driveWheelRadius, - const frc::SimpleMotorFeedforward& steerFF, - const frc::DCMotor& steerMotor, double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, - units::volt_t driveKs, const frc::DCMotor& driveMotor, - double driveGearing, units::meter_t driveWheelRadius, - const frc::LinearSystem<2, 1, 2>& steerPlant, - units::volt_t steerKs, const frc::DCMotor& steerMotor, - double steerGearing, - const frc::SwerveDriveKinematics& kinematics); - void SetDriveInputs(const std::array& inputs); - void SetSteerInputs(const std::array& inputs); + SwerveDriveSim( + const wpi::math::SimpleMotorFeedforward& driveFF, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::SimpleMotorFeedforward& steerFF, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + SwerveDriveSim( + const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs, + const wpi::math::DCMotor& driveMotor, double driveGearing, + wpi::units::meter_t driveWheelRadius, + const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs, + const wpi::math::DCMotor& steerMotor, double steerGearing, + const wpi::math::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); static Eigen::Matrix CalculateX( const Eigen::Matrix& discA, const Eigen::Matrix& discB, - const Eigen::Matrix& x, units::volt_t input, - units::volt_t kS); - void Update(units::second_t dt); - void Reset(const frc::Pose2d& pose, bool preserveMotion); - void Reset(const frc::Pose2d& pose, + const Eigen::Matrix& x, wpi::units::volt_t input, + wpi::units::volt_t kS); + void Update(wpi::units::second_t dt); + void Reset(const wpi::math::Pose2d& pose, bool preserveMotion); + void Reset(const wpi::math::Pose2d& pose, const std::array, numModules>& moduleDriveStates, const std::array, numModules>& moduleSteerStates); - frc::Pose2d GetPose() const; - std::array GetModulePositions() const; - std::array GetNoisyModulePositions( - units::meter_t driveStdDev, units::radian_t steerStdDev); - std::array GetModuleStates(); + wpi::math::Pose2d GetPose() const; + std::array GetModulePositions() + const; + std::array + GetNoisyModulePositions(wpi::units::meter_t driveStdDev, + wpi::units::radian_t steerStdDev); + std::array GetModuleStates(); std::array, numModules> GetDriveStates() const; std::array, numModules> GetSteerStates() const; - units::radians_per_second_t GetOmega() const; - units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, - units::radians_per_second_t velocity, - units::volt_t inputVolts, - units::volt_t batteryVolts) const; - std::array GetDriveCurrentDraw() const; - std::array GetSteerCurrentDraw() const; - units::ampere_t GetTotalCurrentDraw() const; + wpi::units::radians_per_second_t GetOmega() const; + wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor, + wpi::units::radians_per_second_t velocity, + wpi::units::volt_t inputVolts, + wpi::units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + wpi::units::ampere_t GetTotalCurrentDraw() const; private: std::random_device rd{}; std::mt19937 generator{rd()}; std::normal_distribution randDist{0.0, 1.0}; - const frc::LinearSystem<2, 1, 2> drivePlant; - const units::volt_t driveKs; - const frc::DCMotor driveMotor; + const wpi::math::LinearSystem<2, 1, 2> drivePlant; + const wpi::units::volt_t driveKs; + const wpi::math::DCMotor driveMotor; const double driveGearing; - const units::meter_t driveWheelRadius; - const frc::LinearSystem<2, 1, 2> steerPlant; - const units::volt_t steerKs; - const frc::DCMotor steerMotor; + const wpi::units::meter_t driveWheelRadius; + const wpi::math::LinearSystem<2, 1, 2> steerPlant; + const wpi::units::volt_t steerKs; + const wpi::math::DCMotor steerMotor; const double steerGearing; - const frc::SwerveDriveKinematics kinematics; - std::array driveInputs{}; + const wpi::math::SwerveDriveKinematics kinematics; + std::array driveInputs{}; std::array, numModules> driveStates{}; - std::array steerInputs{}; + std::array steerInputs{}; std::array, numModules> steerStates{}; - frc::Pose2d pose{frc::Pose2d{}}; - units::radians_per_second_t omega{0}; + wpi::math::Pose2d pose{wpi::math::Pose2d{}}; + wpi::units::radians_per_second_t omega{0}; }; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h index 9c07ab426d..cb9a9b2df4 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h @@ -24,13 +24,13 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "Constants.h" @@ -38,44 +38,44 @@ class SwerveModule { public: explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); void Periodic(); - void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop, - bool steerInPlace); - frc::Rotation2d GetAbsoluteHeading() const; - frc::SwerveModuleState GetState() const; - frc::SwerveModulePosition GetPosition() const; - units::volt_t GetDriveVoltage() const; - units::volt_t GetSteerVoltage() const; - units::ampere_t GetDriveCurrentSim() const; - units::ampere_t GetSteerCurrentSim() const; + void SetDesiredState(wpi::math::SwerveModuleState newState, + bool shouldBeOpenLoop, bool steerInPlace); + wpi::math::Rotation2d GetAbsoluteHeading() const; + wpi::math::SwerveModuleState GetState() const; + wpi::math::SwerveModulePosition GetPosition() const; + wpi::units::volt_t GetDriveVoltage() const; + wpi::units::volt_t GetSteerVoltage() const; + wpi::units::ampere_t GetDriveCurrentSim() const; + wpi::units::ampere_t GetSteerCurrentSim() const; constants::Swerve::ModuleConstants GetModuleConstants() const; void Log(); - void SimulationUpdate(units::meter_t driveEncoderDist, - units::meters_per_second_t driveEncoderRate, - units::ampere_t driveCurrent, - units::radian_t steerEncoderDist, - units::radians_per_second_t steerEncoderRate, - units::ampere_t steerCurrent); + void SimulationUpdate(wpi::units::meter_t driveEncoderDist, + wpi::units::meters_per_second_t driveEncoderRate, + wpi::units::ampere_t driveCurrent, + wpi::units::radian_t steerEncoderDist, + wpi::units::radians_per_second_t steerEncoderRate, + wpi::units::ampere_t steerCurrent); private: const constants::Swerve::ModuleConstants moduleConstants; - frc::PWMSparkMax driveMotor; - frc::Encoder driveEncoder; - frc::PWMSparkMax steerMotor; - frc::Encoder steerEncoder; + wpi::PWMSparkMax driveMotor; + wpi::Encoder driveEncoder; + wpi::PWMSparkMax steerMotor; + wpi::Encoder steerEncoder; - frc::SwerveModuleState desiredState{}; + wpi::math::SwerveModuleState desiredState{}; bool openLoop{false}; - frc::PIDController drivePIDController{constants::Swerve::kDriveKP, - constants::Swerve::kDriveKI, - constants::Swerve::kDriveKD}; - frc::PIDController steerPIDController{constants::Swerve::kSteerKP, - constants::Swerve::kSteerKI, - constants::Swerve::kSteerKD}; + wpi::math::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + wpi::math::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; - frc::sim::EncoderSim driveEncoderSim; - units::ampere_t driveCurrentSim{0}; - frc::sim::EncoderSim steerEncoderSim; - units::ampere_t steerCurrentSim{0}; + wpi::sim::EncoderSim driveEncoderSim; + wpi::units::ampere_t driveCurrentSim{0}; + wpi::sim::EncoderSim steerEncoderSim; + wpi::units::ampere_t steerCurrentSim{0}; }; diff --git a/photonlib-cpp-examples/poseest/src/test/cpp/main.cpp b/photonlib-cpp-examples/poseest/src/test/cpp/main.cpp index 351c8d3378..6b8949b2f2 100644 --- a/photonlib-cpp-examples/poseest/src/test/cpp/main.cpp +++ b/photonlib-cpp-examples/poseest/src/test/cpp/main.cpp @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include +#include #include "gtest/gtest.h" diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java index da4d3d7fa0..d4659dcbd0 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java @@ -24,18 +24,18 @@ package frc.robot; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Units; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; public class Constants { public static class Vision { diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java index f227c28da0..d981b3f2df 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java @@ -24,7 +24,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.RobotBase; +import org.wpilib.opmode.RobotBase; public final class Main { private Main() {} diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java index 2d58a27479..bd352152bf 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java @@ -26,17 +26,17 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drivetrain.SwerveDrive; import org.photonvision.PhotonCamera; import org.photonvision.PhotonUtils; +import org.wpilib.driverstation.XboxController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.opmode.TimedRobot; +import org.wpilib.simulation.BatterySim; +import org.wpilib.simulation.RoboRioSim; +import org.wpilib.smartdashboard.SmartDashboard; public class Robot extends TimedRobot { private SwerveDrive drivetrain; diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java index ba9d9d01cc..242ef623e9 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java @@ -26,13 +26,13 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.smartdashboard.Field2d; public class VisionSim { // Simulation diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 8e3fc97379..7eb5f20567 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -26,22 +26,22 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.OnboardIMU; -import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.*; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.smartdashboard.SmartDashboard; public class SwerveDrive { // Construct the swerve modules with their respective constants. diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index 07bff21152..ad8bcdaa82 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -24,27 +24,26 @@ package frc.robot.subsystems.drivetrain; -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.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.Discretization; -import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Random; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.Discretization; +import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.util.Nat; +import org.wpilib.system.RobotController; /** * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users @@ -55,8 +54,8 @@ *

In this class, distances are expressed with meters, angles with radians, and inputs with * voltages. * - *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on - * the Sim GUI's field. + *

Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim + * GUI's field. */ public class SwerveDriveSim { private final LinearSystem drivePlant; @@ -201,7 +200,7 @@ public void setDriveInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < driveInputs.length; i++) { double input = inputs[i]; - driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -214,7 +213,7 @@ public void setSteerInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < steerInputs.length; i++) { double input = inputs[i]; - steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -236,7 +235,7 @@ protected static Matrix calculateX( // input required to make next state vel == 0 double inputToStop = nextStateVel / -discB.get(1, 0); // ks effect on system velocity - double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + double ksSystemEffect = Math.clamp(inputToStop, -ks, ks); // after ks system effect: nextStateVel += discB.get(1, 0) * ksSystemEffect; @@ -248,13 +247,13 @@ protected static Matrix calculateX( // system velocity was reduced to 0, resist any input if (Math.abs(ksSystemEffect) < ks) { double absInput = Math.abs(input); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // non-zero system velocity, but input causes velocity sign change. Resist input after sign // change else if ((input * signToStop) > (inputToStop * signToStop)) { double absInput = Math.abs(input - inputToStop); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // calculate next x including static friction @@ -446,8 +445,8 @@ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration - if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); - else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts); + else effVolts = Math.clamp(effVolts, inputVolts, 0); // calculate battery current return (inputVolts / battVolts) * (effVolts / motor.R); } diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index c393321713..ba8bd1daf8 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -26,17 +26,17 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.simulation.EncoderSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.hardware.motor.PWMSparkMax; +import org.wpilib.hardware.rotation.Encoder; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.EncoderSim; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; public class SwerveModule { // --- Module Constants diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java index da4d3d7fa0..d4659dcbd0 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java @@ -24,18 +24,18 @@ package frc.robot; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Units; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; public class Constants { public static class Vision { diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java index f227c28da0..d981b3f2df 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java @@ -24,7 +24,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.RobotBase; +import org.wpilib.opmode.RobotBase; public final class Main { private Main() {} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java index f3c5e24f27..cae762eeb7 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java @@ -26,15 +26,15 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drivetrain.SwerveDrive; import org.photonvision.PhotonCamera; +import org.wpilib.driverstation.XboxController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.opmode.TimedRobot; +import org.wpilib.simulation.BatterySim; +import org.wpilib.simulation.RoboRioSim; +import org.wpilib.smartdashboard.SmartDashboard; public class Robot extends TimedRobot { private SwerveDrive drivetrain; diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java index ba9d9d01cc..242ef623e9 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java @@ -26,13 +26,13 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.smartdashboard.Field2d; public class VisionSim { // Simulation diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 547d237bae..6cb27e0489 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -26,22 +26,22 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.OnboardIMU; -import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.*; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.smartdashboard.SmartDashboard; public class SwerveDrive { // Construct the swerve modules with their respective constants. diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index 07bff21152..ad8bcdaa82 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -24,27 +24,26 @@ package frc.robot.subsystems.drivetrain; -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.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.Discretization; -import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Random; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.Discretization; +import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.util.Nat; +import org.wpilib.system.RobotController; /** * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users @@ -55,8 +54,8 @@ *

In this class, distances are expressed with meters, angles with radians, and inputs with * voltages. * - *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on - * the Sim GUI's field. + *

Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim + * GUI's field. */ public class SwerveDriveSim { private final LinearSystem drivePlant; @@ -201,7 +200,7 @@ public void setDriveInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < driveInputs.length; i++) { double input = inputs[i]; - driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -214,7 +213,7 @@ public void setSteerInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < steerInputs.length; i++) { double input = inputs[i]; - steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -236,7 +235,7 @@ protected static Matrix calculateX( // input required to make next state vel == 0 double inputToStop = nextStateVel / -discB.get(1, 0); // ks effect on system velocity - double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + double ksSystemEffect = Math.clamp(inputToStop, -ks, ks); // after ks system effect: nextStateVel += discB.get(1, 0) * ksSystemEffect; @@ -248,13 +247,13 @@ protected static Matrix calculateX( // system velocity was reduced to 0, resist any input if (Math.abs(ksSystemEffect) < ks) { double absInput = Math.abs(input); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // non-zero system velocity, but input causes velocity sign change. Resist input after sign // change else if ((input * signToStop) > (inputToStop * signToStop)) { double absInput = Math.abs(input - inputToStop); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // calculate next x including static friction @@ -446,8 +445,8 @@ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration - if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); - else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts); + else effVolts = Math.clamp(effVolts, inputVolts, 0); // calculate battery current return (inputVolts / battVolts) * (effVolts / motor.R); } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index c393321713..ba8bd1daf8 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -26,17 +26,17 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.simulation.EncoderSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.hardware.motor.PWMSparkMax; +import org.wpilib.hardware.rotation.Encoder; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.EncoderSim; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; public class SwerveModule { // --- Module Constants diff --git a/photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java b/photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java index 82d1c002db..e10cfdc811 100644 --- a/photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java +++ b/photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java @@ -26,14 +26,14 @@ import static org.junit.jupiter.api.Assertions.fail; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import java.util.List; import org.junit.jupiter.api.Test; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.timesync.TimeSyncSingleton; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; public class JniLoadTest { @Test diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java index 837fd8f81c..71a0ca002f 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java @@ -24,18 +24,18 @@ package frc.robot; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Units; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFields; public class Constants { public static class Vision { diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java index f227c28da0..d981b3f2df 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java @@ -24,7 +24,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.RobotBase; +import org.wpilib.opmode.RobotBase; public final class Main { private Main() {} diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java index 95fac0e8ab..81f0d110a5 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java @@ -24,16 +24,16 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; import frc.robot.subsystems.GamepieceLauncher; import frc.robot.subsystems.drivetrain.SwerveDrive; +import org.wpilib.driverstation.XboxController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.opmode.TimedRobot; +import org.wpilib.simulation.BatterySim; +import org.wpilib.simulation.RoboRioSim; public class Robot extends TimedRobot { private SwerveDrive drivetrain; diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java index 4fe409ab69..ef7118f54a 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java @@ -26,13 +26,6 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -42,6 +35,13 @@ import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.smartdashboard.Field2d; public class Vision { private final PhotonCamera camera; @@ -55,7 +55,7 @@ public class Vision { /** * @param estConsumer Lamba that will accept a pose estimate and pass it to your desired {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} + * org.wpilib.math.estimator.SwerveDrivePoseEstimator} */ public Vision(EstimateConsumer estConsumer) { this.estConsumer = estConsumer; @@ -167,8 +167,8 @@ private void updateEstimationStdDevs( /** * Returns the latest standard deviations of the estimated pose from {@link * #getEstimatedGlobalPose()}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. + * org.wpilib.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only + * be used when there are targets visible. */ public Matrix getEstimationStdDevs() { return curStdDevs; diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java index 93e0fd661c..7afd3ad8e5 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -24,14 +24,13 @@ package frc.robot.subsystems; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.hardware.motor.PWMSparkMax; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.FlywheelSim; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; public class GamepieceLauncher { private final PWMSparkMax motor; @@ -52,7 +51,7 @@ public void setRunning(boolean shouldRun) { public void periodic() { double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed); curMotorCmd = curDesSpd / maxRPM; - curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); + curMotorCmd = Math.clamp(curMotorCmd, 0.0, 1.0); motor.set(curMotorCmd); SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd); diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index cdb04cff63..0b558dd3ec 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -26,22 +26,22 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.OnboardIMU; -import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.*; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.smartdashboard.SmartDashboard; public class SwerveDrive { // Construct the swerve modules with their respective constants. diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java index 07bff21152..ad8bcdaa82 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -24,27 +24,26 @@ package frc.robot.subsystems.drivetrain; -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.VecBuilder; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.Discretization; -import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Random; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.Discretization; +import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.util.Nat; +import org.wpilib.system.RobotController; /** * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users @@ -55,8 +54,8 @@ *

In this class, distances are expressed with meters, angles with radians, and inputs with * voltages. * - *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on - * the Sim GUI's field. + *

Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim + * GUI's field. */ public class SwerveDriveSim { private final LinearSystem drivePlant; @@ -201,7 +200,7 @@ public void setDriveInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < driveInputs.length; i++) { double input = inputs[i]; - driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -214,7 +213,7 @@ public void setSteerInputs(double... inputs) { final double battVoltage = RobotController.getBatteryVoltage(); for (int i = 0; i < steerInputs.length; i++) { double input = inputs[i]; - steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage); } } @@ -236,7 +235,7 @@ protected static Matrix calculateX( // input required to make next state vel == 0 double inputToStop = nextStateVel / -discB.get(1, 0); // ks effect on system velocity - double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + double ksSystemEffect = Math.clamp(inputToStop, -ks, ks); // after ks system effect: nextStateVel += discB.get(1, 0) * ksSystemEffect; @@ -248,13 +247,13 @@ protected static Matrix calculateX( // system velocity was reduced to 0, resist any input if (Math.abs(ksSystemEffect) < ks) { double absInput = Math.abs(input); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // non-zero system velocity, but input causes velocity sign change. Resist input after sign // change else if ((input * signToStop) > (inputToStop * signToStop)) { double absInput = Math.abs(input - inputToStop); - ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput); } // calculate next x including static friction @@ -446,8 +445,8 @@ protected static double getCurrentDraw( DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { double effVolts = inputVolts - radiansPerSec / motor.Kv; // ignore regeneration - if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); - else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts); + else effVolts = Math.clamp(effVolts, inputVolts, 0); // calculate battery current return (inputVolts / battVolts) * (effVolts / motor.R); } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index c393321713..ba8bd1daf8 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -26,17 +26,17 @@ import static frc.robot.Constants.Swerve.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.simulation.EncoderSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.hardware.motor.PWMSparkMax; +import org.wpilib.hardware.rotation.Encoder; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.EncoderSim; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; public class SwerveModule { // --- Module Constants diff --git a/shared/common.gradle b/shared/common.gradle index c13a53ef4f..125d708d48 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -13,7 +13,6 @@ wpilibTools.deps.wpilibVersion = wpilibVersion // See: https://github.com/wpilibsuite/GradleRIO/blob/main/src/main/java/edu/wpi/first/gradlerio/wpi/WPIVersionsExtension.java wpi.getVersions().getOpencvVersion().convention(openCVversion); wpi.getVersions().getWpilibVersion().convention(wpilibVersion); -wpi.getVersions().getWpimathVersion().convention(wpimathVersion); dependencies { implementation project(':photon-targeting') @@ -23,17 +22,17 @@ dependencies { implementation 'org.msgpack:msgpack-core:0.9.0' implementation 'org.msgpack:jackson-dataformat-msgpack:0.9.0' - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("datalog") - implementation wpilibTools.deps.wpilibJava("cameraserver") - implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation wpilibTools.deps.wpilibJava("wpiunits") + implementation "org.wpilib.wpiutil:wpiutil-java:$wpilibVersion" + implementation "org.wpilib.datalog:datalog-java:$wpilibVersion" + implementation "org.wpilib.cameraserver:cameraserver-java:$wpilibVersion" + implementation "org.wpilib.cscore:cscore-java:$wpilibVersion" + implementation "org.wpilib.wpinet:wpinet-java:$wpilibVersion" + implementation "org.wpilib.wpimath:wpimath-java:$wpilibVersion" + implementation "org.wpilib.ntcore:ntcore-java:$wpilibVersion" + implementation "org.wpilib.hal:hal-java:$wpilibVersion" + implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion" + implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion" + implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion" implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/shared/config.gradle b/shared/config.gradle index ebc32468be..7c8b3066ec 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -1,6 +1,5 @@ // Configure Native-Utils WPI Plugin nativeUtils.addWpiNativeUtils() -nativeUtils.withCrossLinuxArm32() nativeUtils.withCrossLinuxArm64() nativeUtils.withCrossSystemCore() diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 9431f5f1fe..480a60d3c9 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -133,17 +133,17 @@ dependencies { implementation project(":photon-targeting") } - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("datalog") - implementation wpilibTools.deps.wpilibJava("cameraserver") - implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation wpilibTools.deps.wpilibJava("wpiunits") + implementation "org.wpilib.wpiutil:wpiutil-java:$wpilibVersion" + implementation "org.wpilib.datalog:datalog-java:$wpilibVersion" + implementation "org.wpilib.cameraserver:cameraserver-java:$wpilibVersion" + implementation "org.wpilib.cscore:cscore-java:$wpilibVersion" + implementation "org.wpilib.wpinet:wpinet-java:$wpilibVersion" + implementation "org.wpilib.wpimath:wpimath-java:$wpilibVersion" + implementation "org.wpilib.ntcore:ntcore-java:$wpilibVersion" + implementation "org.wpilib.hal:hal-java:$wpilibVersion" + implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion" + implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion" + implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion" implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get()