mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Upgrade to wpilib alpha-6 (#2434)
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Ryanforce08 <rradtke1208@gmail.com> Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com> Co-authored-by: Jade Turner <spacey-sooty@proton.me> Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
152
.github/workflows/build.yml
vendored
152
.github/workflows/build.yml
vendored
@@ -23,73 +23,73 @@ jobs:
|
|||||||
|
|
||||||
# build-examples:
|
# build-examples:
|
||||||
|
|
||||||
# strategy:
|
# strategy:
|
||||||
# fail-fast: false
|
# fail-fast: false
|
||||||
# matrix:
|
# matrix:
|
||||||
# include:
|
# include:
|
||||||
# - os: windows-2022
|
# - os: windows-2022
|
||||||
# artifact-name: Win64
|
# artifact-name: Win64
|
||||||
# - os: macos-14
|
# - os: macos-14
|
||||||
# artifact-name: macOS
|
# artifact-name: macOS
|
||||||
# - os: ubuntu-24.04
|
# - os: ubuntu-24.04
|
||||||
# artifact-name: Linux
|
# artifact-name: Linux
|
||||||
|
|
||||||
# name: "Photonlib - Build Examples - ${{ matrix.os }}"
|
# name: "Photonlib - Build Examples - ${{ matrix.os }}"
|
||||||
# runs-on: ${{ matrix.os }}
|
# runs-on: ${{ matrix.os }}
|
||||||
# needs: [build-photonlib-host, build-photonlib-docker]
|
# needs: [build-photonlib-host, build-photonlib-docker]
|
||||||
|
|
||||||
# steps:
|
# steps:
|
||||||
# - name: Checkout code
|
# - name: Checkout code
|
||||||
# uses: actions/checkout@v6
|
# uses: actions/checkout@v6
|
||||||
# with:
|
# with:
|
||||||
# fetch-depth: 0
|
# fetch-depth: 0
|
||||||
# - name: Fetch tags
|
# - name: Fetch tags
|
||||||
# run: git fetch --tags --force
|
# run: git fetch --tags --force
|
||||||
# - uses: actions/setup-java@v5
|
# - uses: actions/setup-java@v5
|
||||||
# with:
|
# with:
|
||||||
# java-version: 25
|
# java-version: 25
|
||||||
# distribution: temurin
|
# distribution: temurin
|
||||||
# - name: Install SystemCore Toolchain
|
# - name: Install SystemCore Toolchain
|
||||||
# run: ./gradlew installSystemCoreToolchain
|
# run: ./gradlew installSystemCoreToolchain
|
||||||
# - name: Delete duplicate toolchains
|
# - name: Delete duplicate toolchains
|
||||||
# run: |
|
# run: |
|
||||||
# find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} +
|
# find ~/.gradle/cache/ -name *bookworm* -exec rm -rf {} +
|
||||||
# du -h . | sort -h
|
# du -h . | sort -h
|
||||||
# if: matrix.os == 'ubuntu-24.04'
|
# if: matrix.os == 'ubuntu-24.04'
|
||||||
# # Download prebuilt photonlib artifacts
|
# # Download prebuilt photonlib artifacts
|
||||||
# - uses: actions/download-artifact@v7
|
# - uses: actions/download-artifact@v7
|
||||||
# with:
|
# with:
|
||||||
# name: maven-${{ matrix.artifact-name }}
|
# name: maven-${{ matrix.artifact-name }}
|
||||||
# - uses: actions/download-artifact@v7
|
# - uses: actions/download-artifact@v7
|
||||||
# with:
|
# with:
|
||||||
# name: maven-Athena
|
# name: maven-SystemCore
|
||||||
# - name: Move to maven local
|
# - name: Move to maven local
|
||||||
# run: |
|
# run: |
|
||||||
# mkdir -p ~/.m2/repository/
|
# mkdir -p ~/.m2/repository/
|
||||||
# mv maven/org ~/.m2/repository/
|
# mv maven/org ~/.m2/repository/
|
||||||
# - name: Copy vendordeps
|
# - name: Copy vendordeps
|
||||||
# shell: bash
|
# shell: bash
|
||||||
# run: |
|
# run: |
|
||||||
# for vendordep_folder in photonlib-*-examples/*/; do
|
# for vendordep_folder in photonlib-*-examples/*/; do
|
||||||
# # Remove trailing slash for cross-platform compatibility
|
# # Remove trailing slash for cross-platform compatibility
|
||||||
# vendordep_folder="${vendordep_folder%/}"
|
# vendordep_folder="${vendordep_folder%/}"
|
||||||
|
|
||||||
# # Filter for projects only
|
# # Filter for projects only
|
||||||
# if [ -e "$vendordep_folder/build.gradle" ]; then
|
# if [ -e "$vendordep_folder/build.gradle" ]; then
|
||||||
# mkdir -p "$vendordep_folder/vendordeps/"
|
# mkdir -p "$vendordep_folder/vendordeps/"
|
||||||
# cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/"
|
# cp vendordeps/photonlib-json-1.0.json "$vendordep_folder/vendordeps/"
|
||||||
# fi
|
# fi
|
||||||
# done
|
# done
|
||||||
# - name: Build Java examples
|
# - name: Build Java examples
|
||||||
# working-directory: photonlib-java-examples
|
# working-directory: photonlib-java-examples
|
||||||
# run: |
|
# run: |
|
||||||
# ./gradlew build
|
# ./gradlew build
|
||||||
# ./gradlew clean
|
# ./gradlew clean
|
||||||
# - name: Build C++ examples
|
# - name: Build C++ examples
|
||||||
# working-directory: photonlib-cpp-examples
|
# working-directory: photonlib-cpp-examples
|
||||||
# run: |
|
# run: |
|
||||||
# ./gradlew build
|
# ./gradlew build
|
||||||
# ./gradlew clean
|
# ./gradlew clean
|
||||||
|
|
||||||
typecheck-client:
|
typecheck-client:
|
||||||
name: "Typecheck Client"
|
name: "Typecheck Client"
|
||||||
@@ -237,12 +237,12 @@ jobs:
|
|||||||
fail-fast: false
|
fail-fast: false
|
||||||
matrix:
|
matrix:
|
||||||
include:
|
include:
|
||||||
# - os: windows-2022
|
- os: windows-2022
|
||||||
# artifact-name: Win64
|
artifact-name: Win64
|
||||||
- os: macos-15 # TODO: Restore to macos-26 with WPILib alpha-6
|
- os: macos-26
|
||||||
artifact-name: macOS
|
artifact-name: macOS
|
||||||
- os: ubuntu-24.04
|
- os: ubuntu-24.04
|
||||||
artifact-name: Linux
|
artifact-name: Linux
|
||||||
|
|
||||||
name: "Photonlib - Build Host - ${{ matrix.artifact-name }}"
|
name: "Photonlib - Build Host - ${{ matrix.artifact-name }}"
|
||||||
runs-on: ${{ matrix.os }}
|
runs-on: ${{ matrix.os }}
|
||||||
@@ -345,7 +345,7 @@ jobs:
|
|||||||
include:
|
include:
|
||||||
- os: ubuntu-24.04
|
- os: ubuntu-24.04
|
||||||
artifact-name: Linux
|
artifact-name: Linux
|
||||||
arch-override: linuxx64
|
arch-override: linuxx86-64
|
||||||
- os: ubuntu-24.04
|
- os: ubuntu-24.04
|
||||||
artifact-name: LinuxArm64
|
artifact-name: LinuxArm64
|
||||||
arch-override: linuxarm64
|
arch-override: linuxarm64
|
||||||
@@ -401,7 +401,7 @@ jobs:
|
|||||||
arch-override: macarm64
|
arch-override: macarm64
|
||||||
- os: macos-latest
|
- os: macos-latest
|
||||||
artifact-name: macOS
|
artifact-name: macOS
|
||||||
arch-override: macx64
|
arch-override: macx86-64
|
||||||
|
|
||||||
runs-on: ${{ matrix.os }}
|
runs-on: ${{ matrix.os }}
|
||||||
name: "Build fat JAR - ${{ matrix.artifact-name }}"
|
name: "Build fat JAR - ${{ matrix.artifact-name }}"
|
||||||
@@ -416,8 +416,8 @@ jobs:
|
|||||||
matrix:
|
matrix:
|
||||||
include:
|
include:
|
||||||
- os: windows-latest
|
- os: windows-latest
|
||||||
artifact-name: Win64
|
artifact-name: Win
|
||||||
arch-override: winx64
|
arch-override: winx86-64
|
||||||
|
|
||||||
runs-on: ${{ matrix.os }}
|
runs-on: ${{ matrix.os }}
|
||||||
name: "Build fat JAR - ${{ matrix.artifact-name }}"
|
name: "Build fat JAR - ${{ matrix.artifact-name }}"
|
||||||
@@ -432,10 +432,10 @@ jobs:
|
|||||||
matrix:
|
matrix:
|
||||||
include:
|
include:
|
||||||
- os: ubuntu-24.04
|
- os: ubuntu-24.04
|
||||||
artifact-name: photonvision-*-linuxx64.jar
|
artifact-name: photonvision-*-linuxx86-64.jar
|
||||||
extraOpts: -Djdk.lang.Process.launchMechanism=vfork
|
extraOpts: -Djdk.lang.Process.launchMechanism=vfork
|
||||||
- os: windows-latest
|
- os: windows-latest
|
||||||
artifact-name: photonvision-*-winx64.jar
|
artifact-name: photonvision-*-winx86-64.jar
|
||||||
- os: macos-latest
|
- os: macos-latest
|
||||||
artifact-name: photonvision-*-macarm64.jar
|
artifact-name: photonvision-*-macarm64.jar
|
||||||
- os: ubuntu-24.04-arm
|
- os: ubuntu-24.04-arm
|
||||||
|
|||||||
264
.github/workflows/python.yml
vendored
264
.github/workflows/python.yml
vendored
@@ -12,134 +12,136 @@ concurrency:
|
|||||||
cancel-in-progress: true
|
cancel-in-progress: true
|
||||||
|
|
||||||
jobs:
|
jobs:
|
||||||
# build-py:
|
build-py:
|
||||||
# runs-on: ubuntu-24.04
|
runs-on: ubuntu-24.04
|
||||||
#
|
|
||||||
# steps:
|
steps:
|
||||||
# - name: Checkout code
|
- name: Checkout code
|
||||||
# uses: actions/checkout@v6
|
uses: actions/checkout@v6
|
||||||
# with:
|
with:
|
||||||
# fetch-depth: 0
|
fetch-depth: 0
|
||||||
#
|
|
||||||
# - name: Set up Python
|
- name: Set up Python
|
||||||
# uses: actions/setup-python@v6
|
uses: actions/setup-python@v6
|
||||||
# with:
|
with:
|
||||||
# python-version: 3.14
|
python-version: 3.14
|
||||||
#
|
|
||||||
# - name: Install dependencies
|
- name: Install dependencies
|
||||||
# run: |
|
run: |
|
||||||
# python -m pip install --upgrade pip
|
python -m pip install --upgrade pip
|
||||||
# pip install setuptools wheel
|
pip install setuptools wheel
|
||||||
#
|
|
||||||
# - name: Build wheel
|
- name: Build wheel
|
||||||
# working-directory: ./photon-lib/py
|
working-directory: ./photon-lib/py
|
||||||
# run: python setup.py sdist bdist_wheel
|
run: python setup.py sdist bdist_wheel
|
||||||
#
|
|
||||||
# - name: Upload artifacts
|
- name: Upload artifacts
|
||||||
# uses: actions/upload-artifact@v7
|
uses: actions/upload-artifact@v7
|
||||||
# with:
|
with:
|
||||||
# name: dist
|
name: dist
|
||||||
# path: ./photon-lib/py/dist/
|
path: ./photon-lib/py/dist/
|
||||||
#
|
|
||||||
# test-py:
|
test-py:
|
||||||
# needs: build-py
|
needs: build-py
|
||||||
# runs-on: ubuntu-24.04
|
runs-on: ubuntu-24.04
|
||||||
#
|
|
||||||
# steps:
|
steps:
|
||||||
# - name: Checkout code
|
- name: Checkout code
|
||||||
# uses: actions/checkout@v6
|
uses: actions/checkout@v6
|
||||||
# with:
|
with:
|
||||||
# fetch-depth: 0
|
fetch-depth: 0
|
||||||
#
|
|
||||||
# - name: Set up Python
|
- name: Set up Python
|
||||||
# uses: actions/setup-python@v6
|
uses: actions/setup-python@v6
|
||||||
# with:
|
with:
|
||||||
# python-version: 3.14
|
python-version: 3.14
|
||||||
#
|
|
||||||
# - name: Install dependencies
|
- name: Install dependencies
|
||||||
# run: |
|
run: |
|
||||||
# python -m pip install --upgrade pip
|
python -m pip install --upgrade pip
|
||||||
# pip install pytest mypy
|
pip install pytest mypy
|
||||||
#
|
|
||||||
# - name: Download artifacts
|
- name: Download artifacts
|
||||||
# uses: actions/download-artifact@v8
|
uses: actions/download-artifact@v8
|
||||||
# with:
|
with:
|
||||||
# name: dist
|
name: dist
|
||||||
# path: dist/
|
path: dist/
|
||||||
#
|
|
||||||
# - name: Install package
|
- name: Install package
|
||||||
# shell: bash
|
shell: bash
|
||||||
# run: pip install --no-cache-dir dist/*.whl
|
run: pip install --no-cache-dir dist/*.whl
|
||||||
#
|
|
||||||
# - name: Run Unit Tests
|
- name: Run Unit Tests
|
||||||
# shell: bash
|
shell: bash
|
||||||
# run: pytest --import-mode=importlib photon-lib/py/test/
|
run: pytest --import-mode=importlib photon-lib/py/test/
|
||||||
#
|
|
||||||
# - name: Run mypy type checking
|
- name: Run mypy type checking
|
||||||
# run: mypy --show-column-numbers --config-file photon-lib/py/pyproject.toml photon-lib
|
run: mypy --show-column-numbers --config-file photon-lib/py/pyproject.toml photon-lib
|
||||||
#
|
|
||||||
# build-python-examples:
|
# build-python-examples:
|
||||||
# needs: build-py
|
# needs: build-py
|
||||||
# strategy:
|
# strategy:
|
||||||
# matrix:
|
# matrix:
|
||||||
# os: [ubuntu-24.04, windows-2022, macos-14]
|
# os: [ubuntu-24.04, windows-2022, macos-14]
|
||||||
# runs-on: ${{ matrix.os }}
|
# runs-on: ${{ matrix.os }}
|
||||||
#
|
|
||||||
# steps:
|
# steps:
|
||||||
#
|
|
||||||
# - name: Checkout code
|
# - name: Checkout code
|
||||||
# uses: actions/checkout@v6
|
# uses: actions/checkout@v6
|
||||||
# with:
|
# with:
|
||||||
# fetch-depth: 0
|
# fetch-depth: 0
|
||||||
#
|
|
||||||
# - name: Set up Python
|
# - name: Set up Python
|
||||||
# uses: actions/setup-python@v6
|
# uses: actions/setup-python@v6
|
||||||
# with:
|
# with:
|
||||||
# python-version: 3.14
|
# python-version: 3.14
|
||||||
#
|
|
||||||
# - name: Install dependencies
|
# - name: Install dependencies
|
||||||
# run: |
|
# run: |
|
||||||
# python -m pip install --upgrade pip
|
# python -m pip install --upgrade pip
|
||||||
#
|
# python -m pip install mypy
|
||||||
# - name: Download artifacts
|
|
||||||
# uses: actions/download-artifact@v8
|
# - name: Download artifacts
|
||||||
# with:
|
# uses: actions/download-artifact@v8
|
||||||
# name: dist
|
# with:
|
||||||
# path: ./photon-lib/py/dist/
|
# name: dist
|
||||||
#
|
# path: ./photon-lib/py/dist/
|
||||||
# - name: Install PhotonLibPy package
|
|
||||||
# working-directory: ./photon-lib/py
|
# - name: Install PhotonLibPy package
|
||||||
# shell: bash
|
# working-directory: ./photon-lib/py
|
||||||
# run: |
|
# shell: bash
|
||||||
# pip install --no-cache-dir dist/*.whl
|
# run: |
|
||||||
#
|
# pip install --no-cache-dir dist/*.whl
|
||||||
# - name: Build Python examples
|
|
||||||
# working-directory: photonlib-python-examples
|
# - name: Build Python examples
|
||||||
# shell: bash
|
# working-directory: photonlib-python-examples
|
||||||
# run: |
|
# shell: bash
|
||||||
# for folder in */;
|
# run: |
|
||||||
# do
|
# for folder in */;
|
||||||
# echo $folder
|
# do
|
||||||
# ./run.sh $folder
|
# echo $folder
|
||||||
# done
|
# ./run.sh $folder
|
||||||
#
|
# done
|
||||||
# deploy:
|
|
||||||
# needs: [test-py, build-python-examples]
|
deploy:
|
||||||
# runs-on: ubuntu-24.04
|
# TODO: BRING BACK PYTHON EXAMPLES
|
||||||
# # Only upload on tags
|
needs: [test-py]
|
||||||
# if: startsWith(github.ref, 'refs/tags/v')
|
runs-on: ubuntu-24.04
|
||||||
#
|
# Only upload on tags
|
||||||
# steps:
|
if: startsWith(github.ref, 'refs/tags/v')
|
||||||
# - name: Download artifacts
|
|
||||||
# uses: actions/download-artifact@v8
|
steps:
|
||||||
# with:
|
- name: Download artifacts
|
||||||
# name: dist
|
uses: actions/download-artifact@v8
|
||||||
# path: dist/
|
with:
|
||||||
#
|
name: dist
|
||||||
# - name: Publish package distributions to PyPI
|
path: dist/
|
||||||
# uses: pypa/gh-action-pypi-publish@release/v1
|
|
||||||
# with:
|
- name: Publish package distributions to PyPI
|
||||||
# packages-dir: ./dist/
|
uses: pypa/gh-action-pypi-publish@release/v1
|
||||||
#
|
with:
|
||||||
# permissions:
|
packages-dir: ./dist/
|
||||||
# id-token: write # IMPORTANT: this permission is mandatory for trusted publishing
|
|
||||||
|
permissions:
|
||||||
|
id-token: write # IMPORTANT: this permission is mandatory for trusted publishing
|
||||||
|
|||||||
@@ -32,11 +32,11 @@ You can run one of the many built in examples straight from the command line, to
|
|||||||
Note that these are case sensitive!
|
Note that these are case sensitive!
|
||||||
|
|
||||||
* `-PArchOverride=foobar`: builds for a target system other than your current architecture. [Valid overrides](https://github.com/wpilibsuite/wpilib-tool-plugin/blob/main/src/main/java/edu/wpi/first/tools/NativePlatforms.java) are:
|
* `-PArchOverride=foobar`: builds for a target system other than your current architecture. [Valid overrides](https://github.com/wpilibsuite/wpilib-tool-plugin/blob/main/src/main/java/edu/wpi/first/tools/NativePlatforms.java) are:
|
||||||
* winx64
|
* winx86-64
|
||||||
* winarm64
|
* winarm64
|
||||||
* macx64
|
* macx86-64
|
||||||
* macarm64
|
* macarm64
|
||||||
* linuxx64
|
* linuxx86-64
|
||||||
* linuxarm64
|
* linuxarm64
|
||||||
* linuxathena
|
* linuxathena
|
||||||
- `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
|
- `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
|
||||||
|
|||||||
24
build.gradle
24
build.gradle
@@ -4,9 +4,9 @@ plugins {
|
|||||||
id "cpp"
|
id "cpp"
|
||||||
id "com.diffplug.spotless" version "8.1.0"
|
id "com.diffplug.spotless" version "8.1.0"
|
||||||
id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
|
id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
|
||||||
id 'org.wpilib.NativeUtils' version '2027.5.1' apply false
|
id 'org.wpilib.NativeUtils' version '2027.7.1' apply false
|
||||||
id 'org.wpilib.DeployUtils' version '2027.1.0' apply false
|
id 'org.wpilib.DeployUtils' version '2027.1.0' apply false
|
||||||
id 'org.photonvision.tools.WpilibTools' version '3.0.0-photon'
|
id 'org.photonvision.tools.WpilibTools' version 'v5.0.1'
|
||||||
id 'com.google.protobuf' version '0.9.5' apply false
|
id 'com.google.protobuf' version '0.9.5' apply false
|
||||||
id 'org.wpilib.GradleJni' version '2027.0.0'
|
id 'org.wpilib.GradleJni' version '2027.0.0'
|
||||||
id "org.ysb33r.doxygen" version "2.0.0" apply false
|
id "org.ysb33r.doxygen" version "2.0.0" apply false
|
||||||
@@ -30,27 +30,21 @@ allprojects {
|
|||||||
ext.localMavenURL = file("$project.buildDir/outputs/maven")
|
ext.localMavenURL = file("$project.buildDir/outputs/maven")
|
||||||
ext.allOutputsFolder = file("$project.buildDir/outputs")
|
ext.allOutputsFolder = file("$project.buildDir/outputs")
|
||||||
|
|
||||||
// Configure the version number.
|
|
||||||
apply from: "versioningHelper.gradle"
|
|
||||||
|
|
||||||
ext {
|
ext {
|
||||||
wpilibVersion = "2027.0.0-alpha-4"
|
wpilibVersion = "2027.0.0-alpha-6"
|
||||||
wpimathVersion = wpilibVersion
|
openCVversion = "2027-4.13.0-3"
|
||||||
openCVYear = "2025"
|
|
||||||
openCVversion = "4.10.0-3"
|
|
||||||
ejmlVersion = "0.43.1";
|
ejmlVersion = "0.43.1";
|
||||||
jacksonVersion = "2.15.2";
|
|
||||||
avajeJsonbVersion = "3.14-RC4";
|
avajeJsonbVersion = "3.14-RC4";
|
||||||
msgpackVersion = "0.9.0";
|
msgpackVersion = "0.9.0";
|
||||||
quickbufVersion = "1.3.3";
|
quickbufVersion = "1.3.3";
|
||||||
jacocoVersion = "0.8.14";
|
jacocoVersion = "0.8.14";
|
||||||
|
|
||||||
javalinVersion = "6.7.0"
|
javalinVersion = "6.7.0"
|
||||||
libcameraDriverVersion = "dev-v2026.0.0-2-gef3d7a0"
|
libcameraDriverVersion = "v2027.0.0-alpha-1"
|
||||||
rknnVersion = "dev-v2026.0.1-1-g89b2888"
|
rknnVersion = "dev-v2026.0.1-3-g14c3ecb"
|
||||||
rubikVersion = "dev-v2026.0.1-4-g13d6279"
|
tfliteVersion = "v2027.0.2-alpha-1"
|
||||||
frcYear = "2027_alpha4"
|
year = "2027_alpha5"
|
||||||
mrcalVersion = "v2027.0.1";
|
mrcalVersion = "v2027.0.2";
|
||||||
|
|
||||||
pubVersion = versionString
|
pubVersion = versionString
|
||||||
isDev = pubVersion.startsWith("dev")
|
isDev = pubVersion.startsWith("dev")
|
||||||
|
|||||||
@@ -25,7 +25,7 @@ Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/re
|
|||||||
:::{note}
|
:::{note}
|
||||||
If your coprocessor has a 64 bit ARM based CPU architecture (OrangePi, Raspberry Pi, etc.), download the LinuxArm64.jar file.
|
If your coprocessor has a 64 bit ARM based CPU architecture (OrangePi, Raspberry Pi, etc.), download the LinuxArm64.jar file.
|
||||||
|
|
||||||
If your coprocessor has an 64 bit x86 based CPU architecture (Mini PC, laptop, etc.), download the Linuxx64.jar file.
|
If your coprocessor has an 64 bit x86 based CPU architecture (Mini PC, laptop, etc.), download the Linuxx86-64.jar file.
|
||||||
:::
|
:::
|
||||||
|
|
||||||
:::{warning}
|
:::{warning}
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ PhotonVision requires a JDK installed and on the system path. **JDK 17 is needed
|
|||||||
|
|
||||||
## Downloading the Latest Stable Release of PhotonVision
|
## Downloading the Latest Stable Release of PhotonVision
|
||||||
|
|
||||||
Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/releases) and download the winx64.jar file.
|
Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/releases) and download the winx86-64.jar file.
|
||||||
|
|
||||||
## Running PhotonVision
|
## Running PhotonVision
|
||||||
|
|
||||||
|
|||||||
@@ -79,9 +79,9 @@ public class Robot extends TimedRobot {
|
|||||||
camera.getAllUnreadResults();
|
camera.getAllUnreadResults();
|
||||||
}
|
}
|
||||||
|
|
||||||
var t1 = Timer.getFPGATimestamp();
|
var t1 = Timer.getMonotonicTimestamp();
|
||||||
light.set(true);
|
light.set(true);
|
||||||
var t2 = Timer.getFPGATimestamp();
|
var t2 = Timer.getMonotonicTimestamp();
|
||||||
|
|
||||||
|
|
||||||
for (int i = 0; i < 100; i++) {
|
for (int i = 0; i < 100; i++) {
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ dependencies {
|
|||||||
wpilibNatives wpilibTools.deps.wpilib("cscore")
|
wpilibNatives wpilibTools.deps.wpilib("cscore")
|
||||||
wpilibNatives wpilibTools.deps.wpilib("apriltag")
|
wpilibNatives wpilibTools.deps.wpilib("apriltag")
|
||||||
wpilibNatives wpilibTools.deps.wpilib("hal")
|
wpilibNatives wpilibTools.deps.wpilib("hal")
|
||||||
wpilibNatives wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion)
|
wpilibNatives wpilibTools.deps.wpilibOpenCv(openCVversion)
|
||||||
|
|
||||||
// These stay as implementation dependencies since they don't have native code that gets packaged
|
// These stay as implementation dependencies since they don't have native code that gets packaged
|
||||||
implementation 'org.zeroturnaround:zt-zip:1.14'
|
implementation 'org.zeroturnaround:zt-zip:1.14'
|
||||||
@@ -41,7 +41,7 @@ dependencies {
|
|||||||
wpilibNatives("org.photonvision:rknn_jni-jni:$rknnVersion:$jniPlatform") {
|
wpilibNatives("org.photonvision:rknn_jni-jni:$rknnVersion:$jniPlatform") {
|
||||||
transitive = false
|
transitive = false
|
||||||
}
|
}
|
||||||
wpilibNatives("org.photonvision:rubik_jni-jni:$rubikVersion:$jniPlatform") {
|
wpilibNatives("org.photonvision:tflite_jni-jni:$tfliteVersion:$jniPlatform") {
|
||||||
transitive = false
|
transitive = false
|
||||||
}
|
}
|
||||||
wpilibNatives("org.photonvision:photon-libcamera-gl-driver-jni:$libcameraDriverVersion:$jniPlatform") {
|
wpilibNatives("org.photonvision:photon-libcamera-gl-driver-jni:$libcameraDriverVersion:$jniPlatform") {
|
||||||
@@ -49,11 +49,17 @@ dependencies {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (jniPlatform == "linuxx86-64") {
|
||||||
|
wpilibNatives("org.photonvision:tflite_jni-jni:$tfliteVersion:$jniPlatform") {
|
||||||
|
transitive = false
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
implementation("org.photonvision:rknn_jni-java:$rknnVersion") {
|
implementation("org.photonvision:rknn_jni-java:$rknnVersion") {
|
||||||
transitive = false
|
transitive = false
|
||||||
}
|
}
|
||||||
|
|
||||||
implementation("org.photonvision:rubik_jni-java:$rubikVersion") {
|
implementation("org.photonvision:tflite_jni-java:$tfliteVersion") {
|
||||||
transitive = false
|
transitive = false
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ public class LoadJNI {
|
|||||||
private static HashMap<JNITypes, Boolean> loadedMap = new HashMap<>();
|
private static HashMap<JNITypes, Boolean> loadedMap = new HashMap<>();
|
||||||
|
|
||||||
public enum JNITypes {
|
public enum JNITypes {
|
||||||
RUBIK_DETECTOR("tensorflowlite", "tensorflowlite_c", "external_delegate", "rubik_jni"),
|
RUBIK_DETECTOR("tflite_jni"),
|
||||||
RKNN_DETECTOR("rga", "rknnrt", "rknn_jni"),
|
RKNN_DETECTOR("rga", "rknnrt", "rknn_jni"),
|
||||||
MRCAL("mrcal_jni"),
|
MRCAL("mrcal_jni"),
|
||||||
LIBCAMERA("photonlibcamera");
|
LIBCAMERA("photonlibcamera");
|
||||||
|
|||||||
@@ -36,7 +36,7 @@ public class NTDataChangeListener {
|
|||||||
this.instance = instance;
|
this.instance = instance;
|
||||||
listenerID =
|
listenerID =
|
||||||
this.instance.addListener(
|
this.instance.addListener(
|
||||||
watchedEntry, EnumSet.of(NetworkTableEvent.Kind.kValueAll), dataChangeConsumer);
|
watchedEntry, EnumSet.of(NetworkTableEvent.Kind.VALUE_ALL), dataChangeConsumer);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void remove() {
|
public void remove() {
|
||||||
|
|||||||
@@ -68,9 +68,9 @@ public class NTDriverStation {
|
|||||||
|
|
||||||
fmsTable.addListener(
|
fmsTable.addListener(
|
||||||
"FMSControlData",
|
"FMSControlData",
|
||||||
EnumSet.of(Kind.kValueAll),
|
EnumSet.of(Kind.VALUE_ALL),
|
||||||
(table, key, event) -> {
|
(table, key, event) -> {
|
||||||
if (event.is(Kind.kValueAll) && event.valueData.value.isInteger()) {
|
if (event.is(Kind.VALUE_ALL) && event.valueData.value.isInteger()) {
|
||||||
// Logger totally isnt thread safe but whatevs
|
// Logger totally isnt thread safe but whatevs
|
||||||
var word = NTDriverStation.getControlWord(event.valueData.value.getInteger());
|
var word = NTDriverStation.getControlWord(event.valueData.value.getInteger());
|
||||||
|
|
||||||
|
|||||||
@@ -86,11 +86,11 @@ public class NetworkTablesManager {
|
|||||||
|
|
||||||
private NetworkTablesManager() {
|
private NetworkTablesManager() {
|
||||||
ntInstance.addLogger(
|
ntInstance.addLogger(
|
||||||
LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages
|
LogMessage.INFO, LogMessage.CRITICAL, this::logNtMessage); // to hide error messages
|
||||||
ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
|
ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
|
||||||
|
|
||||||
ntInstance.addListener(
|
ntInstance.addListener(
|
||||||
m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
|
m_fieldLayoutSubscriber, EnumSet.of(Kind.VALUE_ALL), this::onFieldLayoutChanged);
|
||||||
|
|
||||||
ntDriverStation = new NTDriverStation(this.getNTInst());
|
ntDriverStation = new NTDriverStation(this.getNTInst());
|
||||||
|
|
||||||
@@ -127,16 +127,16 @@ public class NetworkTablesManager {
|
|||||||
private void logNtMessage(NetworkTableEvent event) {
|
private void logNtMessage(NetworkTableEvent event) {
|
||||||
String levelmsg = "DEBUG";
|
String levelmsg = "DEBUG";
|
||||||
LogLevel pvlevel = LogLevel.DEBUG;
|
LogLevel pvlevel = LogLevel.DEBUG;
|
||||||
if (event.logMessage.level >= LogMessage.kCritical) {
|
if (event.logMessage.level >= LogMessage.CRITICAL) {
|
||||||
pvlevel = LogLevel.ERROR;
|
pvlevel = LogLevel.ERROR;
|
||||||
levelmsg = "CRITICAL";
|
levelmsg = "CRITICAL";
|
||||||
} else if (event.logMessage.level >= LogMessage.kError) {
|
} else if (event.logMessage.level >= LogMessage.ERROR) {
|
||||||
pvlevel = LogLevel.ERROR;
|
pvlevel = LogLevel.ERROR;
|
||||||
levelmsg = "ERROR";
|
levelmsg = "ERROR";
|
||||||
} else if (event.logMessage.level >= LogMessage.kWarning) {
|
} else if (event.logMessage.level >= LogMessage.WARNING) {
|
||||||
pvlevel = LogLevel.WARN;
|
pvlevel = LogLevel.WARN;
|
||||||
levelmsg = "WARNING";
|
levelmsg = "WARNING";
|
||||||
} else if (event.logMessage.level >= LogMessage.kInfo) {
|
} else if (event.logMessage.level >= LogMessage.INFO) {
|
||||||
pvlevel = LogLevel.INFO;
|
pvlevel = LogLevel.INFO;
|
||||||
levelmsg = "INFO";
|
levelmsg = "INFO";
|
||||||
}
|
}
|
||||||
@@ -157,16 +157,14 @@ public class NetworkTablesManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public void checkNtConnectState(NetworkTableEvent event) {
|
public void checkNtConnectState(NetworkTableEvent event) {
|
||||||
var isConnEvent = event.is(Kind.kConnected);
|
var isConnEvent = event.is(Kind.CONNECTED);
|
||||||
var isDisconnEvent = event.is(Kind.kDisconnected);
|
var isDisconnEvent = event.is(Kind.DISCONNECTED);
|
||||||
|
|
||||||
if (isDisconnEvent) {
|
if (isDisconnEvent) {
|
||||||
var msg =
|
var msg =
|
||||||
String.format(
|
String.format(
|
||||||
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
|
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
|
||||||
event.connInfo.remote_ip,
|
event.connInfo.remoteIp, event.connInfo.remotePort, event.connInfo.protocolVersion);
|
||||||
event.connInfo.remote_port,
|
|
||||||
event.connInfo.protocol_version);
|
|
||||||
logger.error(msg);
|
logger.error(msg);
|
||||||
HardwareManager.getInstance().setNTConnected(false);
|
HardwareManager.getInstance().setNTConnected(false);
|
||||||
|
|
||||||
@@ -175,9 +173,7 @@ public class NetworkTablesManager {
|
|||||||
var msg =
|
var msg =
|
||||||
String.format(
|
String.format(
|
||||||
"NT connected to %s:%d! (NT version %d)",
|
"NT connected to %s:%d! (NT version %d)",
|
||||||
event.connInfo.remote_ip,
|
event.connInfo.remoteIp, event.connInfo.remotePort, event.connInfo.protocolVersion);
|
||||||
event.connInfo.remote_port,
|
|
||||||
event.connInfo.protocol_version);
|
|
||||||
logger.info(msg);
|
logger.info(msg);
|
||||||
HardwareManager.getInstance().setNTConnected(true);
|
HardwareManager.getInstance().setNTConnected(true);
|
||||||
|
|
||||||
@@ -226,7 +222,7 @@ public class NetworkTablesManager {
|
|||||||
if (ntInstance.isConnected()) {
|
if (ntInstance.isConnected()) {
|
||||||
var connections = ntInstance.getConnections();
|
var connections = ntInstance.getConnections();
|
||||||
if (connections.length > 0) {
|
if (connections.length > 0) {
|
||||||
subMap.put("address", connections[0].remote_ip + ":" + connections[0].remote_port);
|
subMap.put("address", connections[0].remoteIp + ":" + connections[0].remotePort);
|
||||||
}
|
}
|
||||||
subMap.put("clients", connections.length);
|
subMap.put("clients", connections.length);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -112,7 +112,7 @@ public class TimeSyncManager {
|
|||||||
var conns = ntInstance.getConnections();
|
var conns = ntInstance.getConnections();
|
||||||
|
|
||||||
if (conns.length > 0) {
|
if (conns.length > 0) {
|
||||||
var newServer = conns[0].remote_ip;
|
var newServer = conns[0].remoteIp;
|
||||||
if (!m_client.getServer().equals(newServer)) {
|
if (!m_client.getServer().equals(newServer)) {
|
||||||
logger.debug("Changing TimeSyncClient server to " + newServer);
|
logger.debug("Changing TimeSyncClient server to " + newServer);
|
||||||
m_client.setServer(newServer);
|
m_client.setServer(newServer);
|
||||||
|
|||||||
@@ -267,8 +267,8 @@ public class NetworkUtils {
|
|||||||
// Use NT client IP address to find the interface in use
|
// Use NT client IP address to find the interface in use
|
||||||
if (!config.runNTServer) {
|
if (!config.runNTServer) {
|
||||||
var conn = NetworkTableInstance.getDefault().getConnections();
|
var conn = NetworkTableInstance.getDefault().getConnections();
|
||||||
if (conn.length > 0 && !conn[0].remote_ip.equals("127.0.0.1")) {
|
if (conn.length > 0 && !conn[0].remoteIp.equals("127.0.0.1")) {
|
||||||
var addr = InetAddress.getByName(conn[0].remote_ip);
|
var addr = InetAddress.getByName(conn[0].remoteIp);
|
||||||
return formatMacAddress(NetworkInterface.getByInetAddress(addr).getHardwareAddress());
|
return formatMacAddress(NetworkInterface.getByInetAddress(addr).getHardwareAddress());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,83 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) Photon Vision.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.photonvision.common.util.file;
|
|
||||||
|
|
||||||
import io.avaje.json.JsonAdapter;
|
|
||||||
import io.avaje.json.JsonReader;
|
|
||||||
import io.avaje.json.JsonWriter;
|
|
||||||
import io.avaje.json.PropertyNames;
|
|
||||||
import io.avaje.jsonb.CustomAdapter;
|
|
||||||
import io.avaje.jsonb.Json;
|
|
||||||
import io.avaje.jsonb.Jsonb;
|
|
||||||
import io.avaje.jsonb.Types;
|
|
||||||
import java.util.List;
|
|
||||||
import org.wpilib.vision.apriltag.AprilTag;
|
|
||||||
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
|
|
||||||
|
|
||||||
@Json.Import(AprilTag.class)
|
|
||||||
@CustomAdapter
|
|
||||||
public class AprilTagFieldLayoutJsonAdapter implements JsonAdapter<AprilTagFieldLayout> {
|
|
||||||
@Json
|
|
||||||
record FieldDimensions(double length, double width) {}
|
|
||||||
|
|
||||||
JsonAdapter<List<AprilTag>> aprilTagListJsonAdapter;
|
|
||||||
JsonAdapter<FieldDimensions> fieldDimensionsJsonAdapter;
|
|
||||||
PropertyNames names;
|
|
||||||
|
|
||||||
public AprilTagFieldLayoutJsonAdapter(Jsonb jsonb) {
|
|
||||||
aprilTagListJsonAdapter = jsonb.adapter(Types.listOf(AprilTag.class));
|
|
||||||
fieldDimensionsJsonAdapter = jsonb.adapter(FieldDimensions.class);
|
|
||||||
names = jsonb.properties("tags", "field");
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void toJson(JsonWriter writer, AprilTagFieldLayout value) {
|
|
||||||
writer.beginObject(names);
|
|
||||||
writer.name(0);
|
|
||||||
aprilTagListJsonAdapter.toJson(writer, value.getTags());
|
|
||||||
writer.name(1);
|
|
||||||
fieldDimensionsJsonAdapter.toJson(
|
|
||||||
writer, new FieldDimensions(value.getFieldLength(), value.getFieldWidth()));
|
|
||||||
writer.endObject();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public AprilTagFieldLayout fromJson(JsonReader reader) {
|
|
||||||
List<AprilTag> tags = null;
|
|
||||||
FieldDimensions field = null;
|
|
||||||
|
|
||||||
reader.beginObject();
|
|
||||||
while (reader.hasNextField()) {
|
|
||||||
final String fieldName = reader.nextField();
|
|
||||||
switch (fieldName) {
|
|
||||||
case "tags":
|
|
||||||
tags = aprilTagListJsonAdapter.fromJson(reader);
|
|
||||||
break;
|
|
||||||
case "field":
|
|
||||||
field = fieldDimensionsJsonAdapter.fromJson(reader);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
reader.unmappedField(fieldName);
|
|
||||||
reader.skipValue();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
reader.endObject();
|
|
||||||
|
|
||||||
return new AprilTagFieldLayout(tags, field.length, field.width);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,103 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) Photon Vision.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.photonvision.common.util.file;
|
|
||||||
|
|
||||||
import io.avaje.json.JsonAdapter;
|
|
||||||
import io.avaje.json.JsonReader;
|
|
||||||
import io.avaje.json.JsonWriter;
|
|
||||||
import io.avaje.json.PropertyNames;
|
|
||||||
import io.avaje.json.view.ViewBuilder;
|
|
||||||
import io.avaje.json.view.ViewBuilderAware;
|
|
||||||
import io.avaje.jsonb.CustomAdapter;
|
|
||||||
import io.avaje.jsonb.Jsonb;
|
|
||||||
import java.lang.invoke.MethodHandle;
|
|
||||||
import org.wpilib.math.geometry.Pose3d;
|
|
||||||
import org.wpilib.math.geometry.Rotation3d;
|
|
||||||
import org.wpilib.math.geometry.Translation3d;
|
|
||||||
|
|
||||||
@CustomAdapter
|
|
||||||
public class Pose3dJsonAdapter implements JsonAdapter<Pose3d>, ViewBuilderAware {
|
|
||||||
private final JsonAdapter<Translation3d> translation3dJsonAdapter;
|
|
||||||
private final JsonAdapter<Rotation3d> rotation3dJsonAdapter;
|
|
||||||
private final PropertyNames names;
|
|
||||||
|
|
||||||
public Pose3dJsonAdapter(Jsonb jsonb) {
|
|
||||||
this.translation3dJsonAdapter = jsonb.adapter(Translation3d.class);
|
|
||||||
this.rotation3dJsonAdapter = jsonb.adapter(Rotation3d.class);
|
|
||||||
this.names = jsonb.properties("translation", "rotation");
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void toJson(JsonWriter writer, Pose3d value) {
|
|
||||||
writer.beginObject(names);
|
|
||||||
writer.name(0);
|
|
||||||
translation3dJsonAdapter.toJson(writer, value.getTranslation());
|
|
||||||
writer.name(1);
|
|
||||||
rotation3dJsonAdapter.toJson(writer, value.getRotation());
|
|
||||||
writer.endObject();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public Pose3d fromJson(JsonReader reader) {
|
|
||||||
Translation3d translation = null;
|
|
||||||
Rotation3d rotation = null;
|
|
||||||
|
|
||||||
reader.beginObject(names);
|
|
||||||
while (reader.hasNextField()) {
|
|
||||||
final String fieldName = reader.nextField();
|
|
||||||
switch (fieldName) {
|
|
||||||
case "translation":
|
|
||||||
translation = translation3dJsonAdapter.fromJson(reader);
|
|
||||||
break;
|
|
||||||
case "rotation":
|
|
||||||
rotation = rotation3dJsonAdapter.fromJson(reader);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
reader.unmappedField(fieldName);
|
|
||||||
reader.skipValue();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
reader.endObject();
|
|
||||||
|
|
||||||
return new Pose3d(translation, rotation);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isViewBuilderAware() {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public ViewBuilderAware viewBuild() {
|
|
||||||
return this;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void build(ViewBuilder builder, String name, MethodHandle handle) {
|
|
||||||
builder.beginObject(name, handle);
|
|
||||||
builder.add(
|
|
||||||
"translation",
|
|
||||||
translation3dJsonAdapter,
|
|
||||||
builder.method(Pose3d.class, "getTranslation", Translation3d.class));
|
|
||||||
builder.add(
|
|
||||||
"rotation",
|
|
||||||
rotation3dJsonAdapter,
|
|
||||||
builder.method(Pose3d.class, "getRotation", Rotation3d.class));
|
|
||||||
builder.endObject();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,45 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) Photon Vision.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.photonvision.common.util.file;
|
|
||||||
|
|
||||||
import io.avaje.jsonb.Json;
|
|
||||||
import org.wpilib.math.geometry.Quaternion;
|
|
||||||
|
|
||||||
@Json.MixIn(Quaternion.class)
|
|
||||||
public class QuaternionMixIn {
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("W")
|
|
||||||
double m_w;
|
|
||||||
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("X")
|
|
||||||
double m_x;
|
|
||||||
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("Y")
|
|
||||||
double m_y;
|
|
||||||
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("Z")
|
|
||||||
double m_z;
|
|
||||||
|
|
||||||
@Json.Creator
|
|
||||||
public static Quaternion construct(double m_w, double m_x, double m_y, double m_z) {
|
|
||||||
return new Quaternion(m_w, m_x, m_y, m_z);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,34 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) Photon Vision.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.photonvision.common.util.file;
|
|
||||||
|
|
||||||
import io.avaje.jsonb.Json;
|
|
||||||
import org.wpilib.math.geometry.Quaternion;
|
|
||||||
import org.wpilib.math.geometry.Rotation3d;
|
|
||||||
|
|
||||||
@Json.MixIn(Rotation3d.class)
|
|
||||||
public class Rotation3dMixIn {
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("quaternion")
|
|
||||||
Quaternion m_q;
|
|
||||||
|
|
||||||
@Json.Creator
|
|
||||||
public static Rotation3d construct(Quaternion m_q) {
|
|
||||||
return new Rotation3d(m_q);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,41 +0,0 @@
|
|||||||
/*
|
|
||||||
* Copyright (C) Photon Vision.
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.photonvision.common.util.file;
|
|
||||||
|
|
||||||
import io.avaje.jsonb.Json;
|
|
||||||
import org.wpilib.math.geometry.Translation3d;
|
|
||||||
|
|
||||||
@Json.MixIn(Translation3d.class)
|
|
||||||
public class Translation3dMixIn {
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("x")
|
|
||||||
double m_x;
|
|
||||||
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("y")
|
|
||||||
double m_y;
|
|
||||||
|
|
||||||
@Json.Ignore(deserialize = true)
|
|
||||||
@Json.Property("z")
|
|
||||||
double m_z;
|
|
||||||
|
|
||||||
@Json.Creator
|
|
||||||
public static Translation3d construct(double m_x, double m_y, double m_z) {
|
|
||||||
return new Translation3d(m_x, m_y, m_z);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -109,7 +109,7 @@ public class FileVisionSource extends VisionSource {
|
|||||||
this.frameStaticProperties = frameStaticProperties;
|
this.frameStaticProperties = frameStaticProperties;
|
||||||
videoMode =
|
videoMode =
|
||||||
new VideoMode(
|
new VideoMode(
|
||||||
PixelFormat.kMJPEG,
|
PixelFormat.MJPEG,
|
||||||
frameStaticProperties.imageWidth,
|
frameStaticProperties.imageWidth,
|
||||||
frameStaticProperties.imageHeight,
|
frameStaticProperties.imageHeight,
|
||||||
30);
|
30);
|
||||||
|
|||||||
@@ -289,8 +289,8 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
|
|||||||
try {
|
try {
|
||||||
for (VideoMode videoMode : camera.enumerateVideoModes()) {
|
for (VideoMode videoMode : camera.enumerateVideoModes()) {
|
||||||
// Filter grey modes
|
// Filter grey modes
|
||||||
if (videoMode.pixelFormat == PixelFormat.kGray
|
if (videoMode.pixelFormat == PixelFormat.GRAY
|
||||||
|| videoMode.pixelFormat == PixelFormat.kUnknown) {
|
|| videoMode.pixelFormat == PixelFormat.UNKNOWN) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -75,18 +75,18 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
|
|||||||
|
|
||||||
if (sensorModel == LibCameraJNI.SensorModel.IMX219) {
|
if (sensorModel == LibCameraJNI.SensorModel.IMX219) {
|
||||||
// Settings for the IMX219 sensor, which is used on the Pi Camera Module v2
|
// Settings for the IMX219 sensor, which is used on the Pi Camera Module v2
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 120, 120, .39));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 120, 120, .39));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 30, 30, .39));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 30, 30, .39));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 65, 90, .39));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 65, 90, .39));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 30, 30, .39));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 30, 30, .39));
|
||||||
// TODO: fix 1280x720 in the native code and re-add it
|
// TODO: fix 1280x720 in the native code and re-add it
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, .53));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1920, 1080, 15, 20, .53));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 2, 2464 / 2, 15, 20, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 3280 / 2, 2464 / 2, 15, 20, 1));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 4, 2464 / 4, 15, 20, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 3280 / 4, 2464 / 4, 15, 20, 1));
|
||||||
} else if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
|
} else if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
|
||||||
// Taken from https://www.ovt.com/wp-content/uploads/2022/01/OV9281-OV9282-PB-v1.3-WEB.pdf
|
// Taken from https://www.ovt.com/wp-content/uploads/2022/01/OV9281-OV9282-PB-v1.3-WEB.pdf
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 400, 120, 240, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 400, 120, 240, 1));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 800, 120, 120, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1280, 800, 120, 120, 1));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (sensorModel == LibCameraJNI.SensorModel.IMX477) {
|
if (sensorModel == LibCameraJNI.SensorModel.IMX477) {
|
||||||
@@ -101,13 +101,13 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Settings for the OV5647 sensor, which is used by the Pi Camera Module v1
|
// Settings for the OV5647 sensor, which is used by the Pi Camera Module v1
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 90, 90, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 320, 240, 90, 90, 1));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 85, 90, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 640, 480, 85, 90, 1));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 960, 720, 45, 49, 0.74));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 960, 720, 45, 49, 0.74));
|
||||||
// Half the size of the active areas on the OV5647
|
// Half the size of the active areas on the OV5647
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 2592 / 2, 1944 / 2, 20, 20, 1));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 2592 / 2, 1944 / 2, 20, 20, 1));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 720, 30, 45, 0.91));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1280, 720, 30, 45, 0.91));
|
||||||
videoModes.add(new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, 0.72));
|
videoModes.add(new FPSRatedVideoMode(PixelFormat.UNKNOWN, 1920, 1080, 15, 20, 0.72));
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO need to add more video modes for new sensors here
|
// TODO need to add more video modes for new sensors here
|
||||||
|
|||||||
@@ -29,7 +29,7 @@ import org.photonvision.common.logging.LogGroup;
|
|||||||
import org.photonvision.common.logging.Logger;
|
import org.photonvision.common.logging.Logger;
|
||||||
import org.photonvision.vision.frame.StaticFrames;
|
import org.photonvision.vision.frame.StaticFrames;
|
||||||
import org.photonvision.vision.opencv.CVMat;
|
import org.photonvision.vision.opencv.CVMat;
|
||||||
import org.wpilib.driverstation.DriverStation.MatchType;
|
import org.wpilib.driverstation.MatchType;
|
||||||
import org.wpilib.networktables.IntegerEntry;
|
import org.wpilib.networktables.IntegerEntry;
|
||||||
import org.wpilib.networktables.IntegerSubscriber;
|
import org.wpilib.networktables.IntegerSubscriber;
|
||||||
import org.wpilib.networktables.NetworkTable;
|
import org.wpilib.networktables.NetworkTable;
|
||||||
@@ -165,7 +165,7 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
|
|||||||
logger.warn("Did not receive event name, defaulting to 'UNKNOWN'");
|
logger.warn("Did not receive event name, defaulting to 'UNKNOWN'");
|
||||||
}
|
}
|
||||||
|
|
||||||
MatchType wpiMatchType = MatchType.None; // Default is to be unknown
|
MatchType wpiMatchType = MatchType.NONE; // Default is to be unknown
|
||||||
if (matchType.value < 0 || matchType.value >= MatchType.values().length) {
|
if (matchType.value < 0 || matchType.value >= MatchType.values().length) {
|
||||||
logger.error("Invalid match type from FMS: " + matchType.value);
|
logger.error("Invalid match type from FMS: " + matchType.value);
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ public class MJPGFrameConsumer implements AutoCloseable {
|
|||||||
private MjpegServer mjpegServer;
|
private MjpegServer mjpegServer;
|
||||||
|
|
||||||
public MJPGFrameConsumer(String sourceName, int width, int height, int port) {
|
public MJPGFrameConsumer(String sourceName, int width, int height, int port) {
|
||||||
this.cvSource = new CvSource(sourceName, PixelFormat.kMJPEG, width, height, 30);
|
this.cvSource = new CvSource(sourceName, PixelFormat.MJPEG, width, height, 30);
|
||||||
|
|
||||||
this.mjpegServer = new MjpegServer("serve_" + cvSource.getName(), port);
|
this.mjpegServer = new MjpegServer("serve_" + cvSource.getName(), port);
|
||||||
mjpegServer.setSource(cvSource);
|
mjpegServer.setSource(cvSource);
|
||||||
|
|||||||
@@ -91,7 +91,7 @@ public class USBFrameProvider extends CpuImageProcessor {
|
|||||||
cameraMode.height,
|
cameraMode.height,
|
||||||
// hard-coded 3 channel
|
// hard-coded 3 channel
|
||||||
cameraMode.width * 3,
|
cameraMode.width * 3,
|
||||||
PixelFormat.kBGR);
|
PixelFormat.BGR);
|
||||||
|
|
||||||
// This is from wpi::nt::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
|
// This is from wpi::nt::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
|
||||||
// Hal::initialize was called
|
// Hal::initialize was called
|
||||||
|
|||||||
@@ -26,7 +26,8 @@ import org.opencv.core.Size;
|
|||||||
import org.photonvision.common.logging.LogGroup;
|
import org.photonvision.common.logging.LogGroup;
|
||||||
import org.photonvision.common.logging.Logger;
|
import org.photonvision.common.logging.Logger;
|
||||||
import org.photonvision.common.util.ColorHelper;
|
import org.photonvision.common.util.ColorHelper;
|
||||||
import org.photonvision.rubik.RubikJNI;
|
import org.photonvision.tflite.TFLiteJNI;
|
||||||
|
import org.photonvision.tflite.TFLiteJNI.TFLiteSource;
|
||||||
import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult;
|
import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult;
|
||||||
|
|
||||||
/** Manages an object detector using the rubik backend. */
|
/** Manages an object detector using the rubik backend. */
|
||||||
@@ -38,7 +39,7 @@ public class RubikObjectDetector implements ObjectDetector {
|
|||||||
private final Cleanable cleanable;
|
private final Cleanable cleanable;
|
||||||
|
|
||||||
private static Runnable cleanupAction(long ptr) {
|
private static Runnable cleanupAction(long ptr) {
|
||||||
return () -> RubikJNI.destroy(ptr);
|
return () -> TFLiteJNI.destroy(ptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Pointer to the native object */
|
/** Pointer to the native object */
|
||||||
@@ -68,8 +69,10 @@ public class RubikObjectDetector implements ObjectDetector {
|
|||||||
// Create the detector
|
// Create the detector
|
||||||
try {
|
try {
|
||||||
ptr =
|
ptr =
|
||||||
RubikJNI.create(
|
TFLiteJNI.create(
|
||||||
model.modelFile.getPath().toString(), model.properties.version().ordinal());
|
model.modelFile.getPath().toString(),
|
||||||
|
model.properties.version().ordinal(),
|
||||||
|
TFLiteSource.RUBIK.value());
|
||||||
} catch (Exception e) {
|
} catch (Exception e) {
|
||||||
logger.error("Failed to create detector from path " + model.modelFile.getPath(), e);
|
logger.error("Failed to create detector from path " + model.modelFile.getPath(), e);
|
||||||
throw new RuntimeException(
|
throw new RuntimeException(
|
||||||
@@ -83,7 +86,7 @@ public class RubikObjectDetector implements ObjectDetector {
|
|||||||
+ ". Please ensure the model is valid and compatible with the Rubik backend.");
|
+ ". Please ensure the model is valid and compatible with the Rubik backend.");
|
||||||
throw new RuntimeException(
|
throw new RuntimeException(
|
||||||
"Failed to create detector from path " + model.modelFile.getPath());
|
"Failed to create detector from path " + model.modelFile.getPath());
|
||||||
} else if (!RubikJNI.isQuantized(ptr)) {
|
} else if (!TFLiteJNI.isQuantized(ptr)) {
|
||||||
throw new UnsupportedOperationException("Model must be quantized.");
|
throw new UnsupportedOperationException("Model must be quantized.");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -131,7 +134,7 @@ public class RubikObjectDetector implements ObjectDetector {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Detect objects in the letterboxed frame
|
// Detect objects in the letterboxed frame
|
||||||
var results = RubikJNI.detect(ptr, letterboxed.getNativeObjAddr(), boxThresh, nmsThresh);
|
var results = TFLiteJNI.detect(ptr, letterboxed.getNativeObjAddr(), boxThresh, nmsThresh);
|
||||||
|
|
||||||
letterboxed.release();
|
letterboxed.release();
|
||||||
|
|
||||||
|
|||||||
@@ -31,10 +31,10 @@ public class CalculateFPSPipe
|
|||||||
@Override
|
@Override
|
||||||
protected Integer process(Void in) {
|
protected Integer process(Void in) {
|
||||||
if (lastTime < 0) {
|
if (lastTime < 0) {
|
||||||
lastTime = Timer.getFPGATimestamp();
|
lastTime = Timer.getMonotonicTimestamp();
|
||||||
}
|
}
|
||||||
|
|
||||||
var now = Timer.getFPGATimestamp();
|
var now = Timer.getMonotonicTimestamp();
|
||||||
var dtSeconds = now - lastTime;
|
var dtSeconds = now - lastTime;
|
||||||
lastTime = now;
|
lastTime = now;
|
||||||
|
|
||||||
|
|||||||
@@ -604,9 +604,8 @@ public class VisionModule {
|
|||||||
internalMap.put(
|
internalMap.put(
|
||||||
"pixelFormat",
|
"pixelFormat",
|
||||||
((videoMode instanceof LibcameraGpuSource.FPSRatedVideoMode)
|
((videoMode instanceof LibcameraGpuSource.FPSRatedVideoMode)
|
||||||
? "kPicam"
|
? "Picam"
|
||||||
: videoMode.pixelFormat.toString())
|
: videoMode.pixelFormat.toString()));
|
||||||
.substring(1)); // Remove the k prefix
|
|
||||||
|
|
||||||
temp.add(internalMap);
|
temp.add(internalMap);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -36,8 +36,8 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
|
|||||||
import org.photonvision.common.util.TestUtils;
|
import org.photonvision.common.util.TestUtils;
|
||||||
import org.photonvision.jni.LibraryLoader;
|
import org.photonvision.jni.LibraryLoader;
|
||||||
import org.photonvision.vision.frame.provider.FileFrameProvider;
|
import org.photonvision.vision.frame.provider.FileFrameProvider;
|
||||||
import org.wpilib.driverstation.DriverStation;
|
import org.wpilib.driverstation.MatchType;
|
||||||
import org.wpilib.driverstation.DriverStation.MatchType;
|
import org.wpilib.driverstation.internal.DriverStationBackend;
|
||||||
import org.wpilib.hardware.hal.HAL;
|
import org.wpilib.hardware.hal.HAL;
|
||||||
import org.wpilib.networktables.NetworkTableInstance;
|
import org.wpilib.networktables.NetworkTableInstance;
|
||||||
import org.wpilib.simulation.DriverStationSim;
|
import org.wpilib.simulation.DriverStationSim;
|
||||||
@@ -103,7 +103,7 @@ public class FileSaveFrameConsumerTest {
|
|||||||
DriverStationSim.setMatchType(matchType);
|
DriverStationSim.setMatchType(matchType);
|
||||||
DriverStationSim.setMatchNumber(matchNumber);
|
DriverStationSim.setMatchNumber(matchNumber);
|
||||||
DriverStationSim.setEventName(eventName);
|
DriverStationSim.setEventName(eventName);
|
||||||
DriverStation.refreshData();
|
DriverStationBackend.refreshData();
|
||||||
|
|
||||||
// WHEN we save the image
|
// WHEN we save the image
|
||||||
var currentTime = new Date();
|
var currentTime = new Date();
|
||||||
|
|||||||
@@ -17,8 +17,6 @@ apply plugin: 'org.wpilib.NativeUtils'
|
|||||||
apply from: "${rootDir}/shared/config.gradle"
|
apply from: "${rootDir}/shared/config.gradle"
|
||||||
apply from: "${rootDir}/shared/javacommon.gradle"
|
apply from: "${rootDir}/shared/javacommon.gradle"
|
||||||
|
|
||||||
apply from: "${rootDir}/versioningHelper.gradle"
|
|
||||||
|
|
||||||
nativeUtils {
|
nativeUtils {
|
||||||
exportsConfigs {
|
exportsConfigs {
|
||||||
"${nativeName}" {}
|
"${nativeName}" {}
|
||||||
@@ -159,7 +157,7 @@ task generateVendorJson() {
|
|||||||
|
|
||||||
def read = photonlibFileInput.text
|
def read = photonlibFileInput.text
|
||||||
.replace('${photon_version}', pubVersion)
|
.replace('${photon_version}', pubVersion)
|
||||||
.replace('${frc_year}', frcYear)
|
.replace('${year}', year)
|
||||||
photonlibFileOutput.text = read
|
photonlibFileOutput.text = read
|
||||||
|
|
||||||
outputs.upToDateWhen { false }
|
outputs.upToDateWhen { false }
|
||||||
@@ -375,4 +373,4 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
|
|||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion)
|
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv(openCVversion)
|
||||||
|
|||||||
@@ -17,7 +17,7 @@
|
|||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
from wpimath.geometry import Pose3d
|
from wpimath import Pose3d
|
||||||
|
|
||||||
from .targeting.photonTrackedTarget import PhotonTrackedTarget
|
from .targeting.photonTrackedTarget import PhotonTrackedTarget
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
import math
|
import math
|
||||||
|
|
||||||
from wpimath.geometry import Pose3d, Rotation2d, Transform3d
|
from wpimath import Pose3d, Rotation2d, Transform3d
|
||||||
from wpimath.units import meters
|
from wpimath.units import meters
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ from typing import Any
|
|||||||
|
|
||||||
import cv2 as cv
|
import cv2 as cv
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from wpimath.geometry import Rotation3d, Transform3d, Translation3d
|
from wpimath import Rotation3d, Transform3d, Translation3d
|
||||||
|
|
||||||
from ..targeting import PnpResult, TargetCorner
|
from ..targeting import PnpResult, TargetCorner
|
||||||
from .rotTrlTransform3d import RotTrlTransform3d
|
from .rotTrlTransform3d import RotTrlTransform3d
|
||||||
@@ -26,7 +26,7 @@ class OpenCVHelp:
|
|||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
|
def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
|
||||||
return -NWU_TO_EDN + (rot + NWU_TO_EDN)
|
return NWU_TO_EDN.inverse().rotateBy(rot.rotateBy(NWU_TO_EDN))
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
|
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
|
||||||
@@ -147,7 +147,7 @@ class OpenCVHelp:
|
|||||||
in NWU, this would be {0, 0, 1} in EDN.
|
in NWU, this would be {0, 0, 1} in EDN.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
return -EDN_TO_NWU + (rot + EDN_TO_NWU)
|
return EDN_TO_NWU.inverse().rotateBy(rot.rotateBy(EDN_TO_NWU))
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
|
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
from typing import Self
|
from typing import Self
|
||||||
|
|
||||||
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||||
|
|
||||||
|
|
||||||
class RotTrlTransform3d:
|
class RotTrlTransform3d:
|
||||||
@@ -22,7 +22,7 @@ class RotTrlTransform3d:
|
|||||||
|
|
||||||
def inverse(self) -> Self:
|
def inverse(self) -> Self:
|
||||||
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
|
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
|
||||||
invRot = -self.rot
|
invRot = self.rot.inverse()
|
||||||
invTrl = -(self.trl.rotateBy(invRot))
|
invTrl = -(self.trl.rotateBy(invRot))
|
||||||
return type(self)(invRot, invTrl)
|
return type(self)(invRot, invTrl)
|
||||||
|
|
||||||
@@ -42,7 +42,7 @@ class RotTrlTransform3d:
|
|||||||
return trlToApply.rotateBy(self.rot) + self.trl
|
return trlToApply.rotateBy(self.rot) + self.trl
|
||||||
|
|
||||||
def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
|
def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
|
||||||
return rotToApply + self.rot
|
return rotToApply.rotateBy(self.rot)
|
||||||
|
|
||||||
def applyPose(self, poseToApply: Pose3d) -> Pose3d:
|
def applyPose(self, poseToApply: Pose3d) -> Pose3d:
|
||||||
return Pose3d(
|
return Pose3d(
|
||||||
@@ -68,7 +68,9 @@ class RotTrlTransform3d:
|
|||||||
@classmethod
|
@classmethod
|
||||||
def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
|
def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
|
||||||
return cls(
|
return cls(
|
||||||
last.rotation() - initial.rotation(),
|
last.rotation().relativeTo(initial.rotation()),
|
||||||
last.translation()
|
last.translation()
|
||||||
- initial.translation().rotateBy(last.rotation() - initial.rotation()),
|
- initial.translation().rotateBy(
|
||||||
|
last.rotation().relativeTo(initial.rotation())
|
||||||
|
),
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
import math
|
import math
|
||||||
from typing import List, Self
|
from typing import List, Self
|
||||||
|
|
||||||
from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d
|
from wpimath import Pose3d, Rotation2d, Rotation3d, Translation3d
|
||||||
from wpimath.units import meters
|
from wpimath.units import meters
|
||||||
|
|
||||||
from . import RotTrlTransform3d
|
from . import RotTrlTransform3d
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
||||||
from wpimath.geometry import Pose3d, Transform3d, Translation3d
|
from wpimath import Pose3d, Transform3d, Translation3d
|
||||||
|
|
||||||
from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner
|
from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner
|
||||||
from . import OpenCVHelp, TargetModel
|
from . import OpenCVHelp, TargetModel
|
||||||
|
|||||||
@@ -49,7 +49,9 @@ class PhotonPipelineResultSerde:
|
|||||||
ret = Packet()
|
ret = Packet()
|
||||||
|
|
||||||
# metadata is of non-intrinsic type PhotonPipelineMetadata
|
# metadata is of non-intrinsic type PhotonPipelineMetadata
|
||||||
ret.encodeBytes(PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData())
|
ret.encodeBytes(
|
||||||
|
PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
|
||||||
|
)
|
||||||
|
|
||||||
# targets is a custom VLA!
|
# targets is a custom VLA!
|
||||||
ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
|
ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
|
||||||
|
|||||||
@@ -1,5 +1,5 @@
|
|||||||
import ntcore as nt
|
import ntcore as nt
|
||||||
from wpimath.geometry import Transform3d
|
from wpimath import Transform3d
|
||||||
|
|
||||||
from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde
|
from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde
|
||||||
|
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ import struct
|
|||||||
from typing import Generic, Optional, Protocol, TypeVar
|
from typing import Generic, Optional, Protocol, TypeVar
|
||||||
|
|
||||||
import wpilib
|
import wpilib
|
||||||
from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d
|
from wpimath import Quaternion, Rotation3d, Transform3d, Translation3d
|
||||||
|
|
||||||
T = TypeVar("T")
|
T = TypeVar("T")
|
||||||
|
|
||||||
@@ -57,17 +57,16 @@ class Packet:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
def _getNextByteAsInt(self) -> int:
|
def _getNextByteAsInt(self) -> int:
|
||||||
retVal = 0x00
|
|
||||||
|
|
||||||
if not self.outOfBytes:
|
if not self.outOfBytes:
|
||||||
try:
|
try:
|
||||||
retVal = 0x00FF & self.packetData[self.readPos]
|
retVal = 0x00FF & self.packetData[self.readPos]
|
||||||
self.readPos += 1
|
self.readPos += 1
|
||||||
|
return retVal
|
||||||
except IndexError:
|
except IndexError:
|
||||||
wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
|
wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
|
||||||
self.outOfBytes = True
|
self.outOfBytes = True
|
||||||
|
|
||||||
return retVal
|
return 0x00
|
||||||
|
|
||||||
def getData(self) -> bytes:
|
def getData(self) -> bytes:
|
||||||
"""
|
"""
|
||||||
|
|||||||
@@ -118,13 +118,16 @@ class PhotonCamera:
|
|||||||
)
|
)
|
||||||
|
|
||||||
self._prevHeartbeat = 0
|
self._prevHeartbeat = 0
|
||||||
self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
|
self._prevHeartbeatChangeTime = Timer.getMonotonicTimestamp()
|
||||||
|
|
||||||
# Start the time sync server
|
# Start the time sync server
|
||||||
inst.start()
|
inst.start()
|
||||||
|
|
||||||
# Usage reporting
|
# Usage reporting
|
||||||
hal.reportUsage("PhotonVision/PhotonCamera", PhotonCamera.instance_count, "")
|
hal.reportUsage(
|
||||||
|
"PhotonVision/PhotonCamera", # Not 100% sure if this is correct
|
||||||
|
str(PhotonCamera.instance_count),
|
||||||
|
)
|
||||||
PhotonCamera.instance_count += 1
|
PhotonCamera.instance_count += 1
|
||||||
|
|
||||||
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
|
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
|
||||||
@@ -169,7 +172,7 @@ class PhotonCamera:
|
|||||||
|
|
||||||
self._versionCheck()
|
self._versionCheck()
|
||||||
|
|
||||||
now = RobotController.getFPGATime()
|
now = RobotController.getMonotonicTime()
|
||||||
packetWithTimestamp = self._rawBytesEntry.getAtomic()
|
packetWithTimestamp = self._rawBytesEntry.getAtomic()
|
||||||
byteList = packetWithTimestamp.value
|
byteList = packetWithTimestamp.value
|
||||||
packetWithTimestamp.time
|
packetWithTimestamp.time
|
||||||
@@ -297,7 +300,7 @@ class PhotonCamera:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
curHeartbeat = self._heartbeatEntry.get()
|
curHeartbeat = self._heartbeatEntry.get()
|
||||||
now = Timer.getFPGATimestamp()
|
now = Timer.getMonotonicTimestamp()
|
||||||
|
|
||||||
if curHeartbeat != self._prevHeartbeat:
|
if curHeartbeat != self._prevHeartbeat:
|
||||||
self._prevHeartbeat = curHeartbeat
|
self._prevHeartbeat = curHeartbeat
|
||||||
@@ -311,10 +314,10 @@ class PhotonCamera:
|
|||||||
if not _VERSION_CHECK_ENABLED:
|
if not _VERSION_CHECK_ENABLED:
|
||||||
return
|
return
|
||||||
|
|
||||||
if (Timer.getFPGATimestamp() - _lastVersionTimeCheck) < 5.0:
|
if (Timer.getMonotonicTimestamp() - _lastVersionTimeCheck) < 5.0:
|
||||||
return
|
return
|
||||||
|
|
||||||
_lastVersionTimeCheck = Timer.getFPGATimestamp()
|
_lastVersionTimeCheck = Timer.getMonotonicTimestamp()
|
||||||
|
|
||||||
# Heartbeat entry is assumed to always be present. If it's not present, we
|
# Heartbeat entry is assumed to always be present. If it's not present, we
|
||||||
# assume that a camera with that name was never connected in the first place.
|
# assume that a camera with that name was never connected in the first place.
|
||||||
|
|||||||
@@ -21,16 +21,16 @@ import hal
|
|||||||
import wpilib
|
import wpilib
|
||||||
import wpimath.units
|
import wpimath.units
|
||||||
from robotpy_apriltag import AprilTagFieldLayout
|
from robotpy_apriltag import AprilTagFieldLayout
|
||||||
from wpimath.geometry import (
|
from wpimath import (
|
||||||
Pose2d,
|
Pose2d,
|
||||||
Pose3d,
|
Pose3d,
|
||||||
Rotation2d,
|
Rotation2d,
|
||||||
Rotation3d,
|
Rotation3d,
|
||||||
|
TimeInterpolatableRotation2dBuffer,
|
||||||
Transform3d,
|
Transform3d,
|
||||||
Translation2d,
|
Translation2d,
|
||||||
Translation3d,
|
Translation3d,
|
||||||
)
|
)
|
||||||
from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
|
|
||||||
|
|
||||||
from .estimatedRobotPose import EstimatedRobotPose
|
from .estimatedRobotPose import EstimatedRobotPose
|
||||||
from .targeting.photonPipelineResult import PhotonPipelineResult
|
from .targeting.photonPipelineResult import PhotonPipelineResult
|
||||||
@@ -67,7 +67,8 @@ class PhotonPoseEstimator:
|
|||||||
|
|
||||||
# Usage reporting
|
# Usage reporting
|
||||||
hal.reportUsage(
|
hal.reportUsage(
|
||||||
"PhotonVision/PhotonPoseEstimator", PhotonPoseEstimator.instance_count, ""
|
"PhotonVision/PhotonPoseEstimator",
|
||||||
|
str(PhotonPoseEstimator.instance_count),
|
||||||
)
|
)
|
||||||
PhotonPoseEstimator.instance_count += 1
|
PhotonPoseEstimator.instance_count += 1
|
||||||
|
|
||||||
|
|||||||
@@ -6,8 +6,9 @@ import cv2 as cv
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import wpilib
|
import wpilib
|
||||||
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
|
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
|
||||||
from wpimath.geometry import Pose3d, Transform3d
|
from wpimath import Pose3d, Transform3d
|
||||||
from wpimath.units import meters, seconds
|
from wpimath.units import meters, seconds
|
||||||
|
from wpiutil import PixelFormat
|
||||||
|
|
||||||
from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation
|
from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation
|
||||||
from ..estimation.cameraTargetRelation import CameraTargetRelation
|
from ..estimation.cameraTargetRelation import CameraTargetRelation
|
||||||
@@ -66,7 +67,7 @@ class PhotonCameraSim:
|
|||||||
# TODO switch this back to default True when the functionality is enabled
|
# TODO switch this back to default True when the functionality is enabled
|
||||||
self.videoSimProcEnabled: bool = False
|
self.videoSimProcEnabled: bool = False
|
||||||
self.heartbeatCounter: int = 0
|
self.heartbeatCounter: int = 0
|
||||||
self.nextNtEntryTime = wpilib.Timer.getFPGATimestamp()
|
self.nextNtEntryTime = wpilib.Timer.getMonotonicTimestamp()
|
||||||
self.tagLayout = tagLayout
|
self.tagLayout = tagLayout
|
||||||
|
|
||||||
self.cam = camera
|
self.cam = camera
|
||||||
@@ -76,7 +77,7 @@ class PhotonCameraSim:
|
|||||||
# TODO Check fps is right
|
# TODO Check fps is right
|
||||||
self.videoSimRaw = cs.CvSource(
|
self.videoSimRaw = cs.CvSource(
|
||||||
self.cam.getName() + "-raw",
|
self.cam.getName() + "-raw",
|
||||||
cs.VideoMode.PixelFormat.kGray,
|
PixelFormat.GRAY,
|
||||||
self.prop.getResWidth(),
|
self.prop.getResWidth(),
|
||||||
self.prop.getResHeight(),
|
self.prop.getResHeight(),
|
||||||
1,
|
1,
|
||||||
@@ -88,7 +89,7 @@ class PhotonCameraSim:
|
|||||||
# TODO Check fps is right
|
# TODO Check fps is right
|
||||||
self.videoSimProcessed = cs.CvSource(
|
self.videoSimProcessed = cs.CvSource(
|
||||||
self.cam.getName() + "-processed",
|
self.cam.getName() + "-processed",
|
||||||
cs.VideoMode.PixelFormat.kGray,
|
PixelFormat.GRAY,
|
||||||
self.prop.getResWidth(),
|
self.prop.getResWidth(),
|
||||||
self.prop.getResHeight(),
|
self.prop.getResHeight(),
|
||||||
1,
|
1,
|
||||||
@@ -182,7 +183,7 @@ class PhotonCameraSim:
|
|||||||
ready
|
ready
|
||||||
"""
|
"""
|
||||||
# check if this camera is ready for another frame update
|
# check if this camera is ready for another frame update
|
||||||
now = wpilib.Timer.getFPGATimestamp()
|
now = wpilib.Timer.getMonotonicTimestamp()
|
||||||
timestamp = 0.0
|
timestamp = 0.0
|
||||||
iter = 0
|
iter = 0
|
||||||
# prepare next latest update
|
# prepare next latest update
|
||||||
@@ -434,7 +435,7 @@ class PhotonCameraSim:
|
|||||||
|
|
||||||
# put this simulated data to NT
|
# put this simulated data to NT
|
||||||
self.heartbeatCounter += 1
|
self.heartbeatCounter += 1
|
||||||
publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
|
publishTimestampMicros = wpilib.Timer.getMonotonicTimestamp() * 1e6
|
||||||
return PhotonPipelineResult(
|
return PhotonPipelineResult(
|
||||||
ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
|
ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
|
||||||
metadata=PhotonPipelineMetadata(
|
metadata=PhotonPipelineMetadata(
|
||||||
@@ -461,7 +462,7 @@ class PhotonCameraSim:
|
|||||||
:param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency)
|
:param receiveTimestamp: The (sim) timestamp when this result was read by NT in microseconds. If not passed image capture time is assumed be (current time - latency)
|
||||||
"""
|
"""
|
||||||
if receiveTimestamp_us is None:
|
if receiveTimestamp_us is None:
|
||||||
receiveTimestamp_us = wpilib.Timer.getFPGATimestamp() * 1e6
|
receiveTimestamp_us = wpilib.Timer.getMonotonicTimestamp() * 1e6
|
||||||
receiveTimestamp_us = int(receiveTimestamp_us)
|
receiveTimestamp_us = int(receiveTimestamp_us)
|
||||||
|
|
||||||
self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp_us)
|
self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp_us)
|
||||||
|
|||||||
@@ -5,7 +5,7 @@ import typing
|
|||||||
import cv2 as cv
|
import cv2 as cv
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import numpy.typing as npt
|
import numpy.typing as npt
|
||||||
from wpimath.geometry import Rotation2d, Rotation3d, Translation3d
|
from wpimath import Rotation2d, Rotation3d, Translation3d
|
||||||
from wpimath.units import hertz, milliseconds, seconds
|
from wpimath.units import hertz, milliseconds, seconds
|
||||||
|
|
||||||
from ..estimation import RotTrlTransform3d
|
from ..estimation import RotTrlTransform3d
|
||||||
|
|||||||
@@ -3,10 +3,9 @@ import typing
|
|||||||
import wpilib
|
import wpilib
|
||||||
from robotpy_apriltag import AprilTagFieldLayout
|
from robotpy_apriltag import AprilTagFieldLayout
|
||||||
from wpilib import Field2d
|
from wpilib import Field2d
|
||||||
from wpimath.geometry import Pose2d, Pose3d, Transform3d
|
|
||||||
|
|
||||||
# TODO(auscompgeek): update import path when RobotPy re-exports are fixed
|
# TODO(auscompgeek): update import path when RobotPy re-exports are fixed
|
||||||
from wpimath.interpolation._interpolation import TimeInterpolatablePose3dBuffer
|
from wpimath import Pose2d, Pose3d, TimeInterpolatablePose3dBuffer, Transform3d
|
||||||
from wpimath.units import seconds
|
from wpimath.units import seconds
|
||||||
|
|
||||||
from ..estimation import TargetModel
|
from ..estimation import TargetModel
|
||||||
@@ -69,7 +68,7 @@ class VisionSystemSim:
|
|||||||
self.bufferLength
|
self.bufferLength
|
||||||
)
|
)
|
||||||
self.camTrfMap[cameraSim].addSample(
|
self.camTrfMap[cameraSim].addSample(
|
||||||
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
|
wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera
|
||||||
)
|
)
|
||||||
|
|
||||||
def clearCameras(self) -> None:
|
def clearCameras(self) -> None:
|
||||||
@@ -92,7 +91,7 @@ class VisionSystemSim:
|
|||||||
def getRobotToCamera(
|
def getRobotToCamera(
|
||||||
self,
|
self,
|
||||||
cameraSim: PhotonCameraSim,
|
cameraSim: PhotonCameraSim,
|
||||||
time: seconds = wpilib.Timer.getFPGATimestamp(),
|
time: seconds | None = None,
|
||||||
) -> Transform3d | None:
|
) -> Transform3d | None:
|
||||||
"""Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
|
"""Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
|
||||||
empty optional is returned.
|
empty optional is returned.
|
||||||
@@ -102,6 +101,8 @@ class VisionSystemSim:
|
|||||||
|
|
||||||
:returns: The transform of this camera, or an empty optional if it is invalid
|
:returns: The transform of this camera, or an empty optional if it is invalid
|
||||||
"""
|
"""
|
||||||
|
if time is None:
|
||||||
|
time = wpilib.Timer.getMonotonicTimestamp()
|
||||||
if cameraSim in self.camTrfMap:
|
if cameraSim in self.camTrfMap:
|
||||||
trfBuffer = self.camTrfMap[cameraSim]
|
trfBuffer = self.camTrfMap[cameraSim]
|
||||||
sample = trfBuffer.sample(time)
|
sample = trfBuffer.sample(time)
|
||||||
@@ -115,7 +116,7 @@ class VisionSystemSim:
|
|||||||
def getCameraPose(
|
def getCameraPose(
|
||||||
self,
|
self,
|
||||||
cameraSim: PhotonCameraSim,
|
cameraSim: PhotonCameraSim,
|
||||||
time: seconds = wpilib.Timer.getFPGATimestamp(),
|
time: seconds | None = None,
|
||||||
) -> Pose3d | None:
|
) -> Pose3d | None:
|
||||||
"""Get a simulated camera's position on the field. If the requested camera is invalid, an empty
|
"""Get a simulated camera's position on the field. If the requested camera is invalid, an empty
|
||||||
optional is returned.
|
optional is returned.
|
||||||
@@ -124,6 +125,8 @@ class VisionSystemSim:
|
|||||||
|
|
||||||
:returns: The pose of this camera, or an empty optional if it is invalid
|
:returns: The pose of this camera, or an empty optional if it is invalid
|
||||||
"""
|
"""
|
||||||
|
if time is None:
|
||||||
|
time = wpilib.Timer.getMonotonicTimestamp()
|
||||||
robotToCamera = self.getRobotToCamera(cameraSim, time)
|
robotToCamera = self.getRobotToCamera(cameraSim, time)
|
||||||
if robotToCamera is None:
|
if robotToCamera is None:
|
||||||
return None
|
return None
|
||||||
@@ -147,7 +150,7 @@ class VisionSystemSim:
|
|||||||
"""
|
"""
|
||||||
if cameraSim in self.camTrfMap:
|
if cameraSim in self.camTrfMap:
|
||||||
self.camTrfMap[cameraSim].addSample(
|
self.camTrfMap[cameraSim].addSample(
|
||||||
wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
|
wpilib.Timer.getMonotonicTimestamp(), Pose3d() + robotToCamera
|
||||||
)
|
)
|
||||||
return True
|
return True
|
||||||
else:
|
else:
|
||||||
@@ -155,7 +158,7 @@ class VisionSystemSim:
|
|||||||
|
|
||||||
def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
|
def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
|
||||||
"""Reset the transform history for this camera to just the current transform."""
|
"""Reset the transform history for this camera to just the current transform."""
|
||||||
now = wpilib.Timer.getFPGATimestamp()
|
now = wpilib.Timer.getMonotonicTimestamp()
|
||||||
|
|
||||||
def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
|
def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
|
||||||
if cameraSim in self.camTrfMap:
|
if cameraSim in self.camTrfMap:
|
||||||
@@ -240,14 +243,13 @@ class VisionSystemSim:
|
|||||||
currentTargets.remove(target)
|
currentTargets.remove(target)
|
||||||
return removedList
|
return removedList
|
||||||
|
|
||||||
def getRobotPose(
|
def getRobotPose(self, timestamp: seconds | None = None) -> Pose3d | None:
|
||||||
self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
|
|
||||||
) -> Pose3d | None:
|
|
||||||
"""Get the robot pose in meters saved by the vision system at this timestamp.
|
"""Get the robot pose in meters saved by the vision system at this timestamp.
|
||||||
|
|
||||||
:param timestamp: Timestamp of the desired robot pose
|
:param timestamp: Timestamp of the desired robot pose
|
||||||
"""
|
"""
|
||||||
|
if timestamp is None:
|
||||||
|
timestamp = wpilib.Timer.getMonotonicTimestamp()
|
||||||
return self.robotPoseBuffer.sample(timestamp)
|
return self.robotPoseBuffer.sample(timestamp)
|
||||||
|
|
||||||
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
|
def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
|
||||||
@@ -257,7 +259,7 @@ class VisionSystemSim:
|
|||||||
assert type(robotPose) is Pose3d
|
assert type(robotPose) is Pose3d
|
||||||
|
|
||||||
self.robotPoseBuffer.clear()
|
self.robotPoseBuffer.clear()
|
||||||
self.robotPoseBuffer.addSample(wpilib.Timer.getFPGATimestamp(), robotPose)
|
self.robotPoseBuffer.addSample(wpilib.Timer.getMonotonicTimestamp(), robotPose)
|
||||||
|
|
||||||
def getDebugField(self) -> Field2d:
|
def getDebugField(self) -> Field2d:
|
||||||
return self.dbgField
|
return self.dbgField
|
||||||
@@ -280,7 +282,7 @@ class VisionSystemSim:
|
|||||||
self.dbgField.getObject(targetType).setPoses(posesToAdd)
|
self.dbgField.getObject(targetType).setPoses(posesToAdd)
|
||||||
|
|
||||||
# save "real" robot poses over time
|
# save "real" robot poses over time
|
||||||
now = wpilib.Timer.getFPGATimestamp()
|
now = wpilib.Timer.getMonotonicTimestamp()
|
||||||
self.robotPoseBuffer.addSample(now, robotPose)
|
self.robotPoseBuffer.addSample(now, robotPose)
|
||||||
self.dbgField.setRobotPose(robotPose.toPose2d())
|
self.dbgField.setRobotPose(robotPose.toPose2d())
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
import math
|
import math
|
||||||
from typing import overload
|
from typing import overload
|
||||||
|
|
||||||
from wpimath.geometry import Pose3d, Translation3d
|
from wpimath import Pose3d, Translation3d
|
||||||
|
|
||||||
from ..estimation.targetModel import TargetModel
|
from ..estimation.targetModel import TargetModel
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
from typing import TYPE_CHECKING, ClassVar
|
from typing import TYPE_CHECKING, ClassVar
|
||||||
|
|
||||||
from wpimath.geometry import Transform3d
|
from wpimath import Transform3d
|
||||||
|
|
||||||
if TYPE_CHECKING:
|
if TYPE_CHECKING:
|
||||||
from ..generated.MultiTargetPNPResultSerde import MultiTargetPNPResultSerde
|
from ..generated.MultiTargetPNPResultSerde import MultiTargetPNPResultSerde
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
from typing import TYPE_CHECKING, ClassVar
|
from typing import TYPE_CHECKING, ClassVar
|
||||||
|
|
||||||
from wpimath.geometry import Transform3d
|
from wpimath import Transform3d
|
||||||
|
|
||||||
from ..packet import Packet
|
from ..packet import Packet
|
||||||
from .TargetCorner import TargetCorner
|
from .TargetCorner import TargetCorner
|
||||||
|
|||||||
@@ -53,7 +53,7 @@ class TimeSyncServer:
|
|||||||
PORT = 5810
|
PORT = 5810
|
||||||
|
|
||||||
def __init__(self, time_provider: Optional[Callable[[], int]] = None):
|
def __init__(self, time_provider: Optional[Callable[[], int]] = None):
|
||||||
self.time_provider = time_provider or Timer.getFPGATimestamp
|
self.time_provider = time_provider or Timer.getMonotonicTimestamp
|
||||||
self._process: Optional[threading.Thread] = None
|
self._process: Optional[threading.Thread] = None
|
||||||
self.logger = logging.getLogger("PhotonVision-TimeSyncServer")
|
self.logger = logging.getLogger("PhotonVision-TimeSyncServer")
|
||||||
|
|
||||||
|
|||||||
@@ -58,11 +58,11 @@ setup(
|
|||||||
version=versionString,
|
version=versionString,
|
||||||
install_requires=[
|
install_requires=[
|
||||||
"numpy~=2.3",
|
"numpy~=2.3",
|
||||||
"wpilib>=2026.2.1,<2027",
|
"wpilib>=2027.0.0a6",
|
||||||
"robotpy-wpimath>=2026.2.1,<2027",
|
"robotpy-wpimath>=2027.0.0a6",
|
||||||
"robotpy-apriltag>=2026.2.1,<2027",
|
"robotpy-apriltag>=2027.0.0a6",
|
||||||
"robotpy-cscore>=2026.2.1,<2027",
|
"robotpy-cscore>=2027.0.0a6",
|
||||||
"pyntcore>=2026.2.1,<2027",
|
"pyntcore>=2027.0.0a6",
|
||||||
"opencv-python;platform_machine!='roborio'",
|
"opencv-python;platform_machine!='roborio'",
|
||||||
],
|
],
|
||||||
description=descriptionStr,
|
description=descriptionStr,
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ from photonlibpy.estimation import RotTrlTransform3d, TargetModel
|
|||||||
from photonlibpy.estimation.openCVHelp import OpenCVHelp
|
from photonlibpy.estimation.openCVHelp import OpenCVHelp
|
||||||
from photonlibpy.photonCamera import setVersionCheckEnabled
|
from photonlibpy.photonCamera import setVersionCheckEnabled
|
||||||
from photonlibpy.simulation import SimCameraProperties, VisionTargetSim
|
from photonlibpy.simulation import SimCameraProperties, VisionTargetSim
|
||||||
from wpimath.geometry import Pose3d, Rotation3d, Translation3d
|
from wpimath import Pose3d, Rotation3d, Translation3d
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(autouse=True)
|
@pytest.fixture(autouse=True)
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ from photonlibpy.targeting import (
|
|||||||
from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
|
from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
|
||||||
from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
|
from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
|
||||||
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
||||||
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||||
|
|
||||||
|
|
||||||
class PhotonCameraInjector(PhotonCamera):
|
class PhotonCameraInjector(PhotonCamera):
|
||||||
|
|||||||
@@ -25,7 +25,7 @@ from photonlibpy.photonCamera import setVersionCheckEnabled
|
|||||||
def test_roundTrip():
|
def test_roundTrip():
|
||||||
ntcore.NetworkTableInstance.getDefault().stopServer()
|
ntcore.NetworkTableInstance.getDefault().stopServer()
|
||||||
ntcore.NetworkTableInstance.getDefault().setServer("localhost")
|
ntcore.NetworkTableInstance.getDefault().setServer("localhost")
|
||||||
ntcore.NetworkTableInstance.getDefault().startClient4("meme")
|
ntcore.NetworkTableInstance.getDefault().startClient("meme")
|
||||||
|
|
||||||
camera = PhotonCamera("WPI2024")
|
camera = PhotonCamera("WPI2024")
|
||||||
|
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ import numpy as np
|
|||||||
import pytest
|
import pytest
|
||||||
from photonlibpy.estimation import RotTrlTransform3d
|
from photonlibpy.estimation import RotTrlTransform3d
|
||||||
from photonlibpy.simulation import SimCameraProperties
|
from photonlibpy.simulation import SimCameraProperties
|
||||||
from wpimath.geometry import Rotation2d, Translation3d
|
from wpimath import Rotation2d, Translation3d
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture(autouse=True)
|
@pytest.fixture(autouse=True)
|
||||||
|
|||||||
@@ -5,7 +5,7 @@ from photonlibpy.estimation import TargetModel, VisionEstimation
|
|||||||
from photonlibpy.photonCamera import PhotonCamera
|
from photonlibpy.photonCamera import PhotonCamera
|
||||||
from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim
|
from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim
|
||||||
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
||||||
from wpimath.geometry import (
|
from wpimath import (
|
||||||
Pose2d,
|
Pose2d,
|
||||||
Pose3d,
|
Pose3d,
|
||||||
Rotation2d,
|
Rotation2d,
|
||||||
|
|||||||
@@ -3,7 +3,7 @@
|
|||||||
"name": "photonlib",
|
"name": "photonlib",
|
||||||
"version": "${photon_version}",
|
"version": "${photon_version}",
|
||||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
|
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
|
||||||
"frcYear": "${frc_year}",
|
"year": "${year}",
|
||||||
"mavenUrls": [
|
"mavenUrls": [
|
||||||
"https://maven.photonvision.org/repository/internal",
|
"https://maven.photonvision.org/repository/internal",
|
||||||
"https://maven.photonvision.org/repository/snapshots"
|
"https://maven.photonvision.org/repository/snapshots"
|
||||||
|
|||||||
@@ -34,7 +34,7 @@ import org.photonvision.common.networktables.PacketSubscriber;
|
|||||||
import org.photonvision.targeting.PhotonPipelineResult;
|
import org.photonvision.targeting.PhotonPipelineResult;
|
||||||
import org.photonvision.timesync.TimeSyncSingleton;
|
import org.photonvision.timesync.TimeSyncSingleton;
|
||||||
import org.wpilib.driverstation.Alert;
|
import org.wpilib.driverstation.Alert;
|
||||||
import org.wpilib.driverstation.DriverStation;
|
import org.wpilib.driverstation.DriverStationErrors;
|
||||||
import org.wpilib.hardware.hal.HAL;
|
import org.wpilib.hardware.hal.HAL;
|
||||||
import org.wpilib.math.linalg.MatBuilder;
|
import org.wpilib.math.linalg.MatBuilder;
|
||||||
import org.wpilib.math.linalg.Matrix;
|
import org.wpilib.math.linalg.Matrix;
|
||||||
@@ -189,9 +189,12 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
verifyDependencies();
|
verifyDependencies();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/** This is something we only need to check for java because of the way java packages opencv. */
|
||||||
static void verifyDependencies() {
|
static void verifyDependencies() {
|
||||||
// spotless:off
|
// spotless:off
|
||||||
if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) {
|
// WPILIB names their opencv version in the format YEAR-OPENCVVERSION-PATCH
|
||||||
|
// so we split on '-' and take the middle part to get the version number
|
||||||
|
if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion.split("-")[1])) {
|
||||||
String bfw = """
|
String bfw = """
|
||||||
|
|
||||||
|
|
||||||
@@ -232,8 +235,8 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
""";
|
""";
|
||||||
// spotless:on
|
// spotless:on
|
||||||
|
|
||||||
DriverStation.reportWarning(bfw, false);
|
DriverStationErrors.reportWarning(bfw, false);
|
||||||
DriverStation.reportError(bfw, false);
|
DriverStationErrors.reportError(bfw, false);
|
||||||
throw new UnsupportedOperationException(bfw);
|
throw new UnsupportedOperationException(bfw);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -316,10 +319,10 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
timesyncAlert.setText(warningText);
|
timesyncAlert.setText(warningText);
|
||||||
timesyncAlert.set(true);
|
timesyncAlert.set(true);
|
||||||
|
|
||||||
if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
|
if (Timer.getMonotonicTimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
|
||||||
prevTimeSyncWarnTime = Timer.getFPGATimestamp();
|
prevTimeSyncWarnTime = Timer.getMonotonicTimestamp();
|
||||||
|
|
||||||
DriverStation.reportWarning(
|
DriverStationErrors.reportWarning(
|
||||||
warningText
|
warningText
|
||||||
+ "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
|
+ "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
|
||||||
false);
|
false);
|
||||||
@@ -465,7 +468,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
*/
|
*/
|
||||||
public boolean isConnected() {
|
public boolean isConnected() {
|
||||||
var curHeartbeat = heartbeatSubscriber.get();
|
var curHeartbeat = heartbeatSubscriber.get();
|
||||||
var now = Timer.getFPGATimestamp();
|
var now = Timer.getMonotonicTimestamp();
|
||||||
|
|
||||||
if (curHeartbeat < 0) {
|
if (curHeartbeat < 0) {
|
||||||
// we have never heard from the camera
|
// we have never heard from the camera
|
||||||
@@ -518,19 +521,19 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
void verifyVersion() {
|
void verifyVersion() {
|
||||||
if (!VERSION_CHECK_ENABLED) return;
|
if (!VERSION_CHECK_ENABLED) return;
|
||||||
|
|
||||||
if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
|
if ((Timer.getMonotonicTimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
|
||||||
lastVersionCheckTime = Timer.getFPGATimestamp();
|
lastVersionCheckTime = Timer.getMonotonicTimestamp();
|
||||||
|
|
||||||
// Heartbeat entry is assumed to always be present. If it's not present, we
|
// Heartbeat entry is assumed to always be present. If it's not present, we
|
||||||
// assume that a camera with that name was never connected in the first place.
|
// assume that a camera with that name was never connected in the first place.
|
||||||
if (!heartbeatSubscriber.exists()) {
|
if (!heartbeatSubscriber.exists()) {
|
||||||
var cameraNames = getTablesThatLookLikePhotonCameras();
|
var cameraNames = getTablesThatLookLikePhotonCameras();
|
||||||
if (cameraNames.isEmpty()) {
|
if (cameraNames.isEmpty()) {
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
|
"Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
|
||||||
false);
|
false);
|
||||||
} else {
|
} else {
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"PhotonVision coprocessor at path "
|
"PhotonVision coprocessor at path "
|
||||||
+ path
|
+ path
|
||||||
+ " not found on NetworkTables. Double check that your camera names match!",
|
+ " not found on NetworkTables. Double check that your camera names match!",
|
||||||
@@ -543,7 +546,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
cameraNameStr.append("\n");
|
cameraNameStr.append("\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"Found the following PhotonVision cameras on NetworkTables:\n"
|
"Found the following PhotonVision cameras on NetworkTables:\n"
|
||||||
+ cameraNameStr.toString(),
|
+ cameraNameStr.toString(),
|
||||||
false);
|
false);
|
||||||
@@ -551,7 +554,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
}
|
}
|
||||||
// Check for connection status. Warn if disconnected.
|
// Check for connection status. Warn if disconnected.
|
||||||
else if (!isConnected()) {
|
else if (!isConnected()) {
|
||||||
DriverStation.reportWarning(
|
DriverStationErrors.reportWarning(
|
||||||
"PhotonVision coprocessor at path " + path + " is not sending new data.", false);
|
"PhotonVision coprocessor at path " + path + " is not sending new data.", false);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -563,7 +566,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
|
|
||||||
if (remote_uuid == null || remote_uuid.isEmpty()) {
|
if (remote_uuid == null || remote_uuid.isEmpty()) {
|
||||||
// not connected yet?
|
// not connected yet?
|
||||||
DriverStation.reportWarning(
|
DriverStationErrors.reportWarning(
|
||||||
"PhotonVision coprocessor at path "
|
"PhotonVision coprocessor at path "
|
||||||
+ path
|
+ path
|
||||||
+ " has not reported a message interface UUID - is your coprocessor's camera started?",
|
+ " has not reported a message interface UUID - is your coprocessor's camera started?",
|
||||||
@@ -597,7 +600,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
""";
|
""";
|
||||||
// spotless:on
|
// spotless:on
|
||||||
|
|
||||||
DriverStation.reportWarning(bfw, false);
|
DriverStationErrors.reportWarning(bfw, false);
|
||||||
String versionMismatchMessage =
|
String versionMismatchMessage =
|
||||||
"Photon version "
|
"Photon version "
|
||||||
+ PhotonVersion.versionString
|
+ PhotonVersion.versionString
|
||||||
@@ -610,7 +613,7 @@ public class PhotonCamera implements AutoCloseable {
|
|||||||
+ remote_uuid
|
+ remote_uuid
|
||||||
+ ")"
|
+ ")"
|
||||||
+ "!";
|
+ "!";
|
||||||
DriverStation.reportError(versionMismatchMessage, false);
|
DriverStationErrors.reportError(versionMismatchMessage, false);
|
||||||
throw new UnsupportedOperationException(versionMismatchMessage);
|
throw new UnsupportedOperationException(versionMismatchMessage);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -29,7 +29,7 @@ import org.photonvision.estimation.TargetModel;
|
|||||||
import org.photonvision.estimation.VisionEstimation;
|
import org.photonvision.estimation.VisionEstimation;
|
||||||
import org.photonvision.targeting.PhotonPipelineResult;
|
import org.photonvision.targeting.PhotonPipelineResult;
|
||||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||||
import org.wpilib.driverstation.DriverStation;
|
import org.wpilib.driverstation.DriverStationErrors;
|
||||||
import org.wpilib.hardware.hal.HAL;
|
import org.wpilib.hardware.hal.HAL;
|
||||||
import org.wpilib.math.geometry.Pose2d;
|
import org.wpilib.math.geometry.Pose2d;
|
||||||
import org.wpilib.math.geometry.Pose3d;
|
import org.wpilib.math.geometry.Pose3d;
|
||||||
@@ -608,7 +608,7 @@ public class PhotonPoseEstimator {
|
|||||||
return Optional.empty();
|
return Optional.empty();
|
||||||
}
|
}
|
||||||
if (referencePose == null) {
|
if (referencePose == null) {
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
|
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
|
||||||
false);
|
false);
|
||||||
return Optional.empty();
|
return Optional.empty();
|
||||||
@@ -760,7 +760,7 @@ public class PhotonPoseEstimator {
|
|||||||
|
|
||||||
private void reportFiducialPoseError(int fiducialId) {
|
private void reportFiducialPoseError(int fiducialId) {
|
||||||
if (!reportedErrors.contains(fiducialId)) {
|
if (!reportedErrors.contains(fiducialId)) {
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
|
"[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
|
||||||
reportedErrors.add(fiducialId);
|
reportedErrors.add(fiducialId);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -153,7 +153,7 @@ public class PhotonCameraSim implements AutoCloseable {
|
|||||||
|
|
||||||
videoSimRaw =
|
videoSimRaw =
|
||||||
CameraServer.putVideo(camera.getName() + "-raw", prop.getResWidth(), prop.getResHeight());
|
CameraServer.putVideo(camera.getName() + "-raw", prop.getResWidth(), prop.getResHeight());
|
||||||
videoSimRaw.setPixelFormat(PixelFormat.kGray);
|
videoSimRaw.setPixelFormat(PixelFormat.GRAY);
|
||||||
videoSimProcessed =
|
videoSimProcessed =
|
||||||
CameraServer.putVideo(
|
CameraServer.putVideo(
|
||||||
camera.getName() + "-processed", prop.getResWidth(), prop.getResHeight());
|
camera.getName() + "-processed", prop.getResWidth(), prop.getResHeight());
|
||||||
@@ -649,7 +649,7 @@ public class PhotonCameraSim implements AutoCloseable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// put this simulated data to NT
|
// put this simulated data to NT
|
||||||
var now = RobotController.getFPGATime();
|
var now = RobotController.getMonotonicTime();
|
||||||
var ret =
|
var ret =
|
||||||
new PhotonPipelineResult(
|
new PhotonPipelineResult(
|
||||||
heartbeatCounter,
|
heartbeatCounter,
|
||||||
|
|||||||
@@ -41,7 +41,7 @@ import org.opencv.core.Point;
|
|||||||
import org.opencv.imgproc.Imgproc;
|
import org.opencv.imgproc.Imgproc;
|
||||||
import org.photonvision.estimation.OpenCVHelp;
|
import org.photonvision.estimation.OpenCVHelp;
|
||||||
import org.photonvision.estimation.RotTrlTransform3d;
|
import org.photonvision.estimation.RotTrlTransform3d;
|
||||||
import org.wpilib.driverstation.DriverStation;
|
import org.wpilib.driverstation.DriverStationErrors;
|
||||||
import org.wpilib.math.geometry.Pose3d;
|
import org.wpilib.math.geometry.Pose3d;
|
||||||
import org.wpilib.math.geometry.Rotation2d;
|
import org.wpilib.math.geometry.Rotation2d;
|
||||||
import org.wpilib.math.geometry.Rotation3d;
|
import org.wpilib.math.geometry.Rotation3d;
|
||||||
@@ -169,7 +169,7 @@ public class SimCameraProperties {
|
|||||||
public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
|
public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
|
||||||
if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
|
if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
|
||||||
fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179));
|
fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179));
|
||||||
DriverStation.reportError(
|
DriverStationErrors.reportError(
|
||||||
"Requested invalid FOV! Clamping between (1, 179) degrees...", false);
|
"Requested invalid FOV! Clamping between (1, 179) degrees...", false);
|
||||||
}
|
}
|
||||||
double resDiag = Math.hypot(resWidth, resHeight);
|
double resDiag = Math.hypot(resWidth, resHeight);
|
||||||
|
|||||||
@@ -106,7 +106,7 @@ public class VisionSystemSim {
|
|||||||
camTrfMap.put(cameraSim, TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds));
|
camTrfMap.put(cameraSim, TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds));
|
||||||
camTrfMap
|
camTrfMap
|
||||||
.get(cameraSim)
|
.get(cameraSim)
|
||||||
.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera));
|
.addSample(Timer.getMonotonicTimestamp(), new Pose3d().plus(robotToCamera));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -137,7 +137,7 @@ public class VisionSystemSim {
|
|||||||
* @return The transform of this camera, or an empty optional if it is invalid
|
* @return The transform of this camera, or an empty optional if it is invalid
|
||||||
*/
|
*/
|
||||||
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim) {
|
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim) {
|
||||||
return getRobotToCamera(cameraSim, Timer.getFPGATimestamp());
|
return getRobotToCamera(cameraSim, Timer.getMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -164,7 +164,7 @@ public class VisionSystemSim {
|
|||||||
* @return The pose of this camera, or an empty optional if it is invalid
|
* @return The pose of this camera, or an empty optional if it is invalid
|
||||||
*/
|
*/
|
||||||
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim) {
|
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim) {
|
||||||
return getCameraPose(cameraSim, Timer.getFPGATimestamp());
|
return getCameraPose(cameraSim, Timer.getMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -191,7 +191,7 @@ public class VisionSystemSim {
|
|||||||
public boolean adjustCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) {
|
public boolean adjustCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) {
|
||||||
var trfBuffer = camTrfMap.get(cameraSim);
|
var trfBuffer = camTrfMap.get(cameraSim);
|
||||||
if (trfBuffer == null) return false;
|
if (trfBuffer == null) return false;
|
||||||
trfBuffer.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera));
|
trfBuffer.addSample(Timer.getMonotonicTimestamp(), new Pose3d().plus(robotToCamera));
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -207,7 +207,7 @@ public class VisionSystemSim {
|
|||||||
* @return If the cameraSim was valid and transforms were reset
|
* @return If the cameraSim was valid and transforms were reset
|
||||||
*/
|
*/
|
||||||
public boolean resetCameraTransforms(PhotonCameraSim cameraSim) {
|
public boolean resetCameraTransforms(PhotonCameraSim cameraSim) {
|
||||||
double now = Timer.getFPGATimestamp();
|
double now = Timer.getMonotonicTimestamp();
|
||||||
var trfBuffer = camTrfMap.get(cameraSim);
|
var trfBuffer = camTrfMap.get(cameraSim);
|
||||||
if (trfBuffer == null) return false;
|
if (trfBuffer == null) return false;
|
||||||
var lastTrf = new Transform3d(new Pose3d(), trfBuffer.getSample(now).orElse(new Pose3d()));
|
var lastTrf = new Transform3d(new Pose3d(), trfBuffer.getSample(now).orElse(new Pose3d()));
|
||||||
@@ -339,7 +339,7 @@ public class VisionSystemSim {
|
|||||||
* @return The latest robot pose
|
* @return The latest robot pose
|
||||||
*/
|
*/
|
||||||
public Pose3d getRobotPose() {
|
public Pose3d getRobotPose() {
|
||||||
return getRobotPose(Timer.getFPGATimestamp());
|
return getRobotPose(Timer.getMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -368,7 +368,7 @@ public class VisionSystemSim {
|
|||||||
*/
|
*/
|
||||||
public void resetRobotPose(Pose3d robotPose) {
|
public void resetRobotPose(Pose3d robotPose) {
|
||||||
robotPoseBuffer.clear();
|
robotPoseBuffer.clear();
|
||||||
robotPoseBuffer.addSample(Timer.getFPGATimestamp(), robotPose);
|
robotPoseBuffer.addSample(Timer.getMonotonicTimestamp(), robotPose);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Field2d getDebugField() {
|
public Field2d getDebugField() {
|
||||||
@@ -403,7 +403,7 @@ public class VisionSystemSim {
|
|||||||
if (robotPoseMeters == null) return;
|
if (robotPoseMeters == null) return;
|
||||||
|
|
||||||
// save "real" robot poses over time
|
// save "real" robot poses over time
|
||||||
double now = Timer.getFPGATimestamp();
|
double now = Timer.getMonotonicTimestamp();
|
||||||
robotPoseBuffer.addSample(now, robotPoseMeters);
|
robotPoseBuffer.addSample(now, robotPoseMeters);
|
||||||
dbgField.setRobotPose(robotPoseMeters.toPose2d());
|
dbgField.setRobotPose(robotPoseMeters.toPose2d());
|
||||||
|
|
||||||
|
|||||||
@@ -46,51 +46,6 @@
|
|||||||
static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s;
|
static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s;
|
||||||
static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
|
static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
|
||||||
|
|
||||||
inline void verifyDependencies() {
|
|
||||||
if (!(std::string_view{cv::getVersionString()} ==
|
|
||||||
std::string_view{photon::PhotonVersion::opencvTargetVersion})) {
|
|
||||||
std::string bfw =
|
|
||||||
"\n\n\n\n\n"
|
|
||||||
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
|
|
||||||
">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
|
|
||||||
">>> \n"
|
|
||||||
">>> You are running an incompatible version \n"
|
|
||||||
">>> of PhotonVision ! \n"
|
|
||||||
">>> \n"
|
|
||||||
">>> PhotonLib ";
|
|
||||||
bfw += photon::PhotonVersion::versionString;
|
|
||||||
bfw += " is built for OpenCV ";
|
|
||||||
bfw += photon::PhotonVersion::opencvTargetVersion;
|
|
||||||
bfw +=
|
|
||||||
"\n"
|
|
||||||
">>> but you are using OpenCV ";
|
|
||||||
bfw += cv::getVersionString();
|
|
||||||
bfw +=
|
|
||||||
"\n>>> \n"
|
|
||||||
">>> This is neither tested nor supported. \n"
|
|
||||||
">>> You MUST update WPILib, PhotonLib, or both.\n"
|
|
||||||
">>> Check `./gradlew dependencies` and ensure\n"
|
|
||||||
">>> all mentions of OpenCV match the version \n"
|
|
||||||
">>> that PhotonLib was built for. If you find a"
|
|
||||||
">>> a mismatched version in a dependency, you\n"
|
|
||||||
">>> must take steps to update the version of \n"
|
|
||||||
">>> OpenCV used in that dependency. If you do\n"
|
|
||||||
">>> not control that dependency and an updated\n"
|
|
||||||
">>> version is not available, contact the \n"
|
|
||||||
">>> developers of that dependency. \n"
|
|
||||||
">>> \n"
|
|
||||||
">>> Your code will now crash. \n"
|
|
||||||
">>> We hope your day gets better. \n"
|
|
||||||
">>> \n"
|
|
||||||
">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
|
|
||||||
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
|
|
||||||
|
|
||||||
WPILIB_ReportWarning(bfw);
|
|
||||||
WPILIB_ReportError(wpi::err::Error, bfw);
|
|
||||||
throw new std::runtime_error(std::string{bfw});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
|
// bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
|
||||||
// to avoid calling this from static initialization
|
// to avoid calling this from static initialization
|
||||||
static void InitTspServer() {
|
static void InitTspServer() {
|
||||||
@@ -169,7 +124,6 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance,
|
|||||||
"' is disconnected.",
|
"' is disconnected.",
|
||||||
wpi::Alert::Level::MEDIUM),
|
wpi::Alert::Level::MEDIUM),
|
||||||
timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::Level::MEDIUM) {
|
timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::Level::MEDIUM) {
|
||||||
verifyDependencies();
|
|
||||||
InstanceCount++;
|
InstanceCount++;
|
||||||
HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, "");
|
HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, "");
|
||||||
|
|
||||||
@@ -195,7 +149,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
|
|||||||
|
|
||||||
// Fill the packet with latest data and populate result.
|
// Fill the packet with latest data and populate result.
|
||||||
wpi::units::microsecond_t now =
|
wpi::units::microsecond_t now =
|
||||||
wpi::units::microsecond_t(wpi::RobotController::GetFPGATime());
|
wpi::units::microsecond_t(wpi::RobotController::GetMonotonicTime());
|
||||||
const auto value = rawBytesEntry.Get();
|
const auto value = rawBytesEntry.Get();
|
||||||
if (!value.size()) return PhotonPipelineResult{};
|
if (!value.size()) return PhotonPipelineResult{};
|
||||||
|
|
||||||
@@ -265,9 +219,9 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
|
|||||||
timesyncAlert.SetText(warningText);
|
timesyncAlert.SetText(warningText);
|
||||||
timesyncAlert.Set(true);
|
timesyncAlert.Set(true);
|
||||||
|
|
||||||
if (wpi::Timer::GetFPGATimestamp() <
|
if (wpi::Timer::GetMonotonicTimestamp() >
|
||||||
(prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
|
(prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
|
||||||
prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp();
|
prevTimeSyncWarnTime = wpi::Timer::GetMonotonicTimestamp();
|
||||||
|
|
||||||
WPILIB_ReportWarning(
|
WPILIB_ReportWarning(
|
||||||
warningText +
|
warningText +
|
||||||
@@ -325,7 +279,7 @@ const std::string_view PhotonCamera::GetCameraName() const {
|
|||||||
|
|
||||||
bool PhotonCamera::IsConnected() {
|
bool PhotonCamera::IsConnected() {
|
||||||
auto currentHeartbeat = heartbeatSubscriber.Get();
|
auto currentHeartbeat = heartbeatSubscriber.Get();
|
||||||
auto now = wpi::Timer::GetFPGATimestamp();
|
auto now = wpi::Timer::GetMonotonicTimestamp();
|
||||||
|
|
||||||
if (currentHeartbeat < 0) {
|
if (currentHeartbeat < 0) {
|
||||||
// we have never heard from the camera
|
// we have never heard from the camera
|
||||||
@@ -373,10 +327,10 @@ void PhotonCamera::VerifyVersion() {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((wpi::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
|
if ((wpi::Timer::GetMonotonicTimestamp() - lastVersionCheckTime) <
|
||||||
VERSION_CHECK_INTERVAL)
|
VERSION_CHECK_INTERVAL)
|
||||||
return;
|
return;
|
||||||
this->lastVersionCheckTime = wpi::Timer::GetFPGATimestamp();
|
this->lastVersionCheckTime = wpi::Timer::GetMonotonicTimestamp();
|
||||||
|
|
||||||
const std::string& versionString = versionEntry.Get("");
|
const std::string& versionString = versionEntry.Get("");
|
||||||
if (versionString.empty()) {
|
if (versionString.empty()) {
|
||||||
|
|||||||
@@ -51,7 +51,7 @@ PhotonCameraSim::PhotonCameraSim(
|
|||||||
videoSimRaw =
|
videoSimRaw =
|
||||||
wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
|
wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
|
||||||
prop.GetResWidth(), prop.GetResHeight());
|
prop.GetResWidth(), prop.GetResHeight());
|
||||||
videoSimRaw.SetPixelFormat(wpi::util::PixelFormat::kGray);
|
videoSimRaw.SetPixelFormat(wpi::util::PixelFormat::GRAY);
|
||||||
videoSimProcessed = wpi::CameraServer::PutVideo(
|
videoSimProcessed = wpi::CameraServer::PutVideo(
|
||||||
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
|
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
|
||||||
prop.GetResHeight());
|
prop.GetResHeight());
|
||||||
|
|||||||
@@ -30,6 +30,5 @@ extern const char* versionString;
|
|||||||
extern const char* buildDate;
|
extern const char* buildDate;
|
||||||
extern const bool isRelease;
|
extern const bool isRelease;
|
||||||
extern const char* wpilibTargetVersion;
|
extern const char* wpilibTargetVersion;
|
||||||
extern const char* opencvTargetVersion;
|
|
||||||
} // namespace PhotonVersion
|
} // namespace PhotonVersion
|
||||||
} // namespace photon
|
} // namespace photon
|
||||||
|
|||||||
@@ -101,7 +101,7 @@ class VisionSystemSim {
|
|||||||
std::make_pair(std::move(cameraSim),
|
std::make_pair(std::move(cameraSim),
|
||||||
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d>{
|
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d>{
|
||||||
bufferLength}));
|
bufferLength}));
|
||||||
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(),
|
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetMonotonicTimestamp(),
|
||||||
wpi::math::Pose3d{} + robotToCamera);
|
wpi::math::Pose3d{} + robotToCamera);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -138,7 +138,7 @@ class VisionSystemSim {
|
|||||||
*/
|
*/
|
||||||
std::optional<wpi::math::Transform3d> GetRobotToCamera(
|
std::optional<wpi::math::Transform3d> GetRobotToCamera(
|
||||||
PhotonCameraSim* cameraSim) {
|
PhotonCameraSim* cameraSim) {
|
||||||
return GetRobotToCamera(cameraSim, wpi::Timer::GetFPGATimestamp());
|
return GetRobotToCamera(cameraSim, wpi::Timer::GetMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -175,7 +175,7 @@ class VisionSystemSim {
|
|||||||
* @return The pose of this camera, or an empty optional if it is invalid
|
* @return The pose of this camera, or an empty optional if it is invalid
|
||||||
*/
|
*/
|
||||||
std::optional<wpi::math::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
|
std::optional<wpi::math::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
|
||||||
return GetCameraPose(cameraSim, wpi::Timer::GetFPGATimestamp());
|
return GetCameraPose(cameraSim, wpi::Timer::GetMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -207,7 +207,7 @@ class VisionSystemSim {
|
|||||||
bool AdjustCamera(PhotonCameraSim* cameraSim,
|
bool AdjustCamera(PhotonCameraSim* cameraSim,
|
||||||
const wpi::math::Transform3d& robotToCamera) {
|
const wpi::math::Transform3d& robotToCamera) {
|
||||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||||
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(),
|
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetMonotonicTimestamp(),
|
||||||
wpi::math::Pose3d{} + robotToCamera);
|
wpi::math::Pose3d{} + robotToCamera);
|
||||||
return true;
|
return true;
|
||||||
} else {
|
} else {
|
||||||
@@ -230,7 +230,7 @@ class VisionSystemSim {
|
|||||||
* @return If the cameraSim was valid and transforms were reset
|
* @return If the cameraSim was valid and transforms were reset
|
||||||
*/
|
*/
|
||||||
bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
|
bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
|
||||||
wpi::units::second_t now = wpi::Timer::GetFPGATimestamp();
|
wpi::units::second_t now = wpi::Timer::GetMonotonicTimestamp();
|
||||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||||
auto trfBuffer = camTrfMap.at(cameraSim);
|
auto trfBuffer = camTrfMap.at(cameraSim);
|
||||||
wpi::math::Transform3d lastTrf{
|
wpi::math::Transform3d lastTrf{
|
||||||
@@ -369,7 +369,7 @@ class VisionSystemSim {
|
|||||||
* @return The latest robot pose
|
* @return The latest robot pose
|
||||||
*/
|
*/
|
||||||
wpi::math::Pose3d GetRobotPose() {
|
wpi::math::Pose3d GetRobotPose() {
|
||||||
return GetRobotPose(wpi::Timer::GetFPGATimestamp());
|
return GetRobotPose(wpi::Timer::GetMonotonicTimestamp());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -398,7 +398,7 @@ class VisionSystemSim {
|
|||||||
*/
|
*/
|
||||||
void ResetRobotPose(const wpi::math::Pose3d& robotPose) {
|
void ResetRobotPose(const wpi::math::Pose3d& robotPose) {
|
||||||
robotPoseBuffer.Clear();
|
robotPoseBuffer.Clear();
|
||||||
robotPoseBuffer.AddSample(wpi::Timer::GetFPGATimestamp(), robotPose);
|
robotPoseBuffer.AddSample(wpi::Timer::GetMonotonicTimestamp(), robotPose);
|
||||||
}
|
}
|
||||||
wpi::Field2d& GetDebugField() { return dbgField; }
|
wpi::Field2d& GetDebugField() { return dbgField; }
|
||||||
|
|
||||||
@@ -427,7 +427,7 @@ class VisionSystemSim {
|
|||||||
dbgField.GetObject(set.first)->SetPoses(posesToAdd);
|
dbgField.GetObject(set.first)->SetPoses(posesToAdd);
|
||||||
}
|
}
|
||||||
|
|
||||||
wpi::units::second_t now = wpi::Timer::GetFPGATimestamp();
|
wpi::units::second_t now = wpi::Timer::GetMonotonicTimestamp();
|
||||||
robotPoseBuffer.AddSample(now, robotPose);
|
robotPoseBuffer.AddSample(now, robotPose);
|
||||||
dbgField.SetRobotPose(robotPose.ToPose2d());
|
dbgField.SetRobotPose(robotPose.ToPose2d());
|
||||||
|
|
||||||
|
|||||||
@@ -125,7 +125,7 @@ class PhotonCameraTest {
|
|||||||
|
|
||||||
var res = camera.getLatestResult();
|
var res = camera.getLatestResult();
|
||||||
var captureTime = res.getTimestampSeconds();
|
var captureTime = res.getTimestampSeconds();
|
||||||
var now = Timer.getFPGATimestamp();
|
var now = Timer.getMonotonicTimestamp();
|
||||||
|
|
||||||
// expectTrue(captureTime < now);
|
// expectTrue(captureTime < now);
|
||||||
|
|
||||||
|
|||||||
@@ -13,8 +13,6 @@ ext.licenseFile = file("$rootDir/LICENSE")
|
|||||||
apply from: "${rootDir}/shared/config.gradle"
|
apply from: "${rootDir}/shared/config.gradle"
|
||||||
apply from: "${rootDir}/shared/javacommon.gradle"
|
apply from: "${rootDir}/shared/javacommon.gradle"
|
||||||
|
|
||||||
apply from: "${rootDir}/versioningHelper.gradle"
|
|
||||||
|
|
||||||
nativeUtils {
|
nativeUtils {
|
||||||
exportsConfigs {
|
exportsConfigs {
|
||||||
"${nativeName}" {}
|
"${nativeName}" {}
|
||||||
@@ -187,5 +185,5 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
|
|||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, openCVversion)
|
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv(openCVversion)
|
||||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
||||||
|
|||||||
@@ -152,9 +152,9 @@ public enum Platform {
|
|||||||
|
|
||||||
switch (platPath) {
|
switch (platPath) {
|
||||||
case "/linux/x86-64/":
|
case "/linux/x86-64/":
|
||||||
return "linuxx64";
|
return "linuxx86-64";
|
||||||
case "/windows/x86-64/":
|
case "/windows/x86-64/":
|
||||||
return "winx64";
|
return "winx86-64";
|
||||||
case "/linux/arm64/":
|
case "/linux/arm64/":
|
||||||
return "linuxarm64";
|
return "linuxarm64";
|
||||||
default:
|
default:
|
||||||
|
|||||||
@@ -169,8 +169,7 @@ constrained_solvepnp::do_optimization(
|
|||||||
point_observations.cols() != (nTags * 4)) {
|
point_observations.cols() != (nTags * 4)) {
|
||||||
if constexpr (VERBOSE) fmt::println("Got unexpected num cols!");
|
if constexpr (VERBOSE) fmt::println("Got unexpected num cols!");
|
||||||
// TODO find a new error code
|
// TODO find a new error code
|
||||||
return wpi::util::unexpected{
|
return wpi::util::unexpected{slp::ExitStatus::NONFINITE_INITIAL_GUESS};
|
||||||
slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// rescale observations to homogenous pixel coordinates
|
// rescale observations to homogenous pixel coordinates
|
||||||
@@ -199,8 +198,7 @@ constrained_solvepnp::do_optimization(
|
|||||||
|
|
||||||
auto problemOpt = createProblem(nTags, heading_free);
|
auto problemOpt = createProblem(nTags, heading_free);
|
||||||
if (!problemOpt) {
|
if (!problemOpt) {
|
||||||
return wpi::util::unexpected{
|
return wpi::util::unexpected{slp::ExitStatus::NONFINITE_INITIAL_GUESS};
|
||||||
slp::ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
ProblemState<3> pState{robot2camera, field2points, point_observations,
|
ProblemState<3> pState{robot2camera, field2points, point_observations,
|
||||||
|
|||||||
@@ -59,7 +59,7 @@ public class CscoreExtrasTest {
|
|||||||
|
|
||||||
UsbCamera camera = CameraServer.startAutomaticCapture(2);
|
UsbCamera camera = CameraServer.startAutomaticCapture(2);
|
||||||
|
|
||||||
camera.setVideoMode(PixelFormat.kMJPEG, 1280, 720, 30);
|
camera.setVideoMode(PixelFormat.MJPEG, 1280, 720, 30);
|
||||||
var cameraMode = camera.getVideoMode();
|
var cameraMode = camera.getVideoMode();
|
||||||
|
|
||||||
CvSink cvSink = CameraServer.getVideo(camera);
|
CvSink cvSink = CameraServer.getVideo(camera);
|
||||||
@@ -74,7 +74,7 @@ public class CscoreExtrasTest {
|
|||||||
cameraMode.height,
|
cameraMode.height,
|
||||||
// hard-coded 3 channel
|
// hard-coded 3 channel
|
||||||
cameraMode.width * 3,
|
cameraMode.width * 3,
|
||||||
PixelFormat.kBGR);
|
PixelFormat.BGR);
|
||||||
final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0;
|
final double CSCORE_DEFAULT_FRAME_TIMEOUT = 1.0 / 4.0;
|
||||||
long time =
|
long time =
|
||||||
CscoreExtras.grabRawSinkFrameTimeoutLastTime(
|
CscoreExtras.grabRawSinkFrameTimeoutLastTime(
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "cpp"
|
id "cpp"
|
||||||
id "google-test-test-suite"
|
id "google-test-test-suite"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
repositories {
|
repositories {
|
||||||
@@ -11,7 +11,7 @@ repositories {
|
|||||||
|
|
||||||
wpi.maven.useLocal = false
|
wpi.maven.useLocal = false
|
||||||
wpi.maven.useDevelopment = false
|
wpi.maven.useDevelopment = false
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
|
|||||||
@@ -24,6 +24,7 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <frc/simulation/OnboardIMUSim.h>
|
||||||
#include <wpi/hardware/imu/OnboardIMU.hpp>
|
#include <wpi/hardware/imu/OnboardIMU.hpp>
|
||||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||||
@@ -73,8 +74,7 @@ class SwerveDrive {
|
|||||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||||
|
|
||||||
// TODO(Jade) onboard imu doesn't have sim yet
|
wpi::sim::OnboardIMUSim gyroSim;
|
||||||
// wpi::sim::ADXRS450_GyroSim gyroSim;
|
|
||||||
SwerveDriveSim swerveDriveSim;
|
SwerveDriveSim swerveDriveSim;
|
||||||
wpi::units::ampere_t totalCurrentDraw{0};
|
wpi::units::ampere_t totalCurrentDraw{0};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "cpp"
|
id "cpp"
|
||||||
id "google-test-test-suite"
|
id "google-test-test-suite"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
repositories {
|
repositories {
|
||||||
@@ -11,7 +11,7 @@ repositories {
|
|||||||
|
|
||||||
wpi.maven.useLocal = false
|
wpi.maven.useLocal = false
|
||||||
wpi.maven.useDevelopment = false
|
wpi.maven.useDevelopment = false
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
|
|||||||
@@ -26,7 +26,7 @@
|
|||||||
|
|
||||||
#include <frc/ADXRS450_Gyro.h>
|
#include <frc/ADXRS450_Gyro.h>
|
||||||
#include <frc/SPI.h>
|
#include <frc/SPI.h>
|
||||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
#include <frc/simulation/OnboardIMUSim.h>
|
||||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||||
|
|
||||||
@@ -75,7 +75,7 @@ class SwerveDrive {
|
|||||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||||
|
|
||||||
wpi::sim::ADXRS450_GyroSim gyroSim;
|
wpi::sim::OnboardIMUSim gyroSim;
|
||||||
SwerveDriveSim swerveDriveSim;
|
SwerveDriveSim swerveDriveSim;
|
||||||
wpi::units::ampere_t totalCurrentDraw{0};
|
wpi::units::ampere_t totalCurrentDraw{0};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "com.diffplug.spotless" version "8.4.0"
|
id "com.diffplug.spotless" version "8.4.0"
|
||||||
|
id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
|
||||||
}
|
}
|
||||||
|
|
||||||
allprojects {
|
allprojects {
|
||||||
@@ -8,6 +9,7 @@ allprojects {
|
|||||||
mavenCentral()
|
mavenCentral()
|
||||||
mavenLocal()
|
mavenLocal()
|
||||||
maven { url = "https://maven.photonvision.org/releases" }
|
maven { url = "https://maven.photonvision.org/releases" }
|
||||||
|
wpilibRepositories.use2027Repos()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "cpp"
|
id "cpp"
|
||||||
id "google-test-test-suite"
|
id "google-test-test-suite"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
repositories {
|
repositories {
|
||||||
@@ -11,7 +11,7 @@ repositories {
|
|||||||
|
|
||||||
wpi.maven.useLocal = false
|
wpi.maven.useLocal = false
|
||||||
wpi.maven.useDevelopment = false
|
wpi.maven.useDevelopment = false
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
|
|||||||
@@ -26,7 +26,7 @@
|
|||||||
|
|
||||||
#include <frc/ADXRS450_Gyro.h>
|
#include <frc/ADXRS450_Gyro.h>
|
||||||
#include <frc/SPI.h>
|
#include <frc/SPI.h>
|
||||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
#include <frc/simulation/OnboardIMUSim.h>
|
||||||
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
#include <wpi/math/estimator/SwerveDrivePoseEstimator.hpp>
|
||||||
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
|
||||||
|
|
||||||
@@ -80,7 +80,7 @@ class SwerveDrive {
|
|||||||
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
wpi::math::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||||
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
wpi::math::ChassisSpeeds targetChassisSpeeds{};
|
||||||
|
|
||||||
wpi::sim::ADXRS450_GyroSim gyroSim;
|
wpi::sim::OnboardIMUSim gyroSim;
|
||||||
SwerveDriveSim swerveDriveSim;
|
SwerveDriveSim swerveDriveSim;
|
||||||
wpi::units::ampere_t totalCurrentDraw{0};
|
wpi::units::ampere_t totalCurrentDraw{0};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "java"
|
id "java"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
java {
|
java {
|
||||||
@@ -15,7 +15,7 @@ repositories {
|
|||||||
}
|
}
|
||||||
|
|
||||||
wpi.maven.useDevelopment = true
|
wpi.maven.useDevelopment = true
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
@@ -103,7 +103,7 @@ jar {
|
|||||||
from('src') { into 'backup/src' }
|
from('src') { into 'backup/src' }
|
||||||
from('vendordeps') { into 'backup/vendordeps' }
|
from('vendordeps') { into 'backup/vendordeps' }
|
||||||
from('build.gradle') { into 'backup' }
|
from('build.gradle') { into 'backup' }
|
||||||
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
||||||
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "java"
|
id "java"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
java {
|
java {
|
||||||
@@ -11,7 +11,7 @@ java {
|
|||||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||||
|
|
||||||
wpi.maven.useDevelopment = true
|
wpi.maven.useDevelopment = true
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
@@ -99,7 +99,7 @@ jar {
|
|||||||
from('src') { into 'backup/src' }
|
from('src') { into 'backup/src' }
|
||||||
from('vendordeps') { into 'backup/vendordeps' }
|
from('vendordeps') { into 'backup/vendordeps' }
|
||||||
from('build.gradle') { into 'backup' }
|
from('build.gradle') { into 'backup' }
|
||||||
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
||||||
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "com.diffplug.spotless" version "8.4.0"
|
id "com.diffplug.spotless" version "8.4.0"
|
||||||
|
id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
|
||||||
}
|
}
|
||||||
|
|
||||||
allprojects {
|
allprojects {
|
||||||
@@ -8,6 +9,7 @@ allprojects {
|
|||||||
mavenCentral()
|
mavenCentral()
|
||||||
mavenLocal()
|
mavenLocal()
|
||||||
maven { url = "https://maven.photonvision.org/releases" }
|
maven { url = "https://maven.photonvision.org/releases" }
|
||||||
|
wpilibRepositories.use2027Repos()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "java"
|
id "java"
|
||||||
id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
|
id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
java {
|
java {
|
||||||
@@ -11,7 +11,7 @@ java {
|
|||||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||||
|
|
||||||
wpi.maven.useDevelopment = true
|
wpi.maven.useDevelopment = true
|
||||||
wpi.versions.wpilibVersion = "2027.0.0-alpha-4"
|
wpi.versions.wpilibVersion = "2027.0.0-alpha-6"
|
||||||
|
|
||||||
// Define my targets (SystemCore) and artifacts (deployable files)
|
// Define my targets (SystemCore) and artifacts (deployable files)
|
||||||
// This is added by GradleRIO's backing project DeployUtils.
|
// This is added by GradleRIO's backing project DeployUtils.
|
||||||
@@ -99,7 +99,7 @@ jar {
|
|||||||
from('src') { into 'backup/src' }
|
from('src') { into 'backup/src' }
|
||||||
from('vendordeps') { into 'backup/vendordeps' }
|
from('vendordeps') { into 'backup/vendordeps' }
|
||||||
from('build.gradle') { into 'backup' }
|
from('build.gradle') { into 'backup' }
|
||||||
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
||||||
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -41,6 +41,7 @@ import org.wpilib.math.linalg.Matrix;
|
|||||||
import org.wpilib.math.linalg.VecBuilder;
|
import org.wpilib.math.linalg.VecBuilder;
|
||||||
import org.wpilib.math.numbers.*;
|
import org.wpilib.math.numbers.*;
|
||||||
import org.wpilib.math.system.plant.DCMotor;
|
import org.wpilib.math.system.plant.DCMotor;
|
||||||
|
import org.wpilib.simulation.OnboardIMUSim;
|
||||||
import org.wpilib.smartdashboard.SmartDashboard;
|
import org.wpilib.smartdashboard.SmartDashboard;
|
||||||
|
|
||||||
public class SwerveDrive {
|
public class SwerveDrive {
|
||||||
@@ -69,8 +70,7 @@ public class SwerveDrive {
|
|||||||
private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds();
|
private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds();
|
||||||
|
|
||||||
// ----- Simulation
|
// ----- Simulation
|
||||||
// TODO(Jade) WPILib doesn't have onboard IMU sim yet
|
private final OnboardIMUSim gyroSim;
|
||||||
// private final ADXRS450_GyroSim gyroSim;
|
|
||||||
private final SwerveDriveSim swerveDriveSim;
|
private final SwerveDriveSim swerveDriveSim;
|
||||||
private double totalCurrentDraw = 0;
|
private double totalCurrentDraw = 0;
|
||||||
|
|
||||||
@@ -91,7 +91,7 @@ public class SwerveDrive {
|
|||||||
visionStdDevs);
|
visionStdDevs);
|
||||||
|
|
||||||
// ----- Simulation
|
// ----- Simulation
|
||||||
// gyroSim = new ADXRS450_GyroSim(gyro);
|
gyroSim = new OnboardIMUSim(gyro);
|
||||||
swerveDriveSim =
|
swerveDriveSim =
|
||||||
new SwerveDriveSim(
|
new SwerveDriveSim(
|
||||||
kDriveFF,
|
kDriveFF,
|
||||||
@@ -188,8 +188,8 @@ public class SwerveDrive {
|
|||||||
for (int i = 0; i < swerveMods.length; i++) {
|
for (int i = 0; i < swerveMods.length; i++) {
|
||||||
swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0);
|
swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0);
|
||||||
}
|
}
|
||||||
// gyroSim.setAngle(-pose.getRotation().getDegrees());
|
gyroSim.setYaw(-pose.getRotation().getDegrees());
|
||||||
// gyroSim.setRate(0);
|
gyroSim.setRate(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose);
|
poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose);
|
||||||
@@ -312,8 +312,8 @@ public class SwerveDrive {
|
|||||||
drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]);
|
drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
|
gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
|
||||||
// gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees());
|
gyroSim.setYaw(-swerveDriveSim.getPose().getRotation().getDegrees());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -28,8 +28,7 @@ import math
|
|||||||
import swervemodule
|
import swervemodule
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.geometry
|
import wpimath
|
||||||
import wpimath.kinematics
|
|
||||||
|
|
||||||
kMaxSpeed = 3.0 # 3 meters per second
|
kMaxSpeed = 3.0 # 3 meters per second
|
||||||
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
||||||
@@ -41,10 +40,10 @@ class Drivetrain:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
|
self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
|
||||||
self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
|
self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
|
||||||
self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
|
self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
|
||||||
self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
|
self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)
|
||||||
|
|
||||||
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
||||||
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
||||||
@@ -55,16 +54,17 @@ class Drivetrain:
|
|||||||
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
||||||
|
|
||||||
self.gyro = wpilib.AnalogGyro(0)
|
self.gyro = wpilib.AnalogGyro(0)
|
||||||
self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
|
self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro)
|
||||||
|
self._yaw = 0.0
|
||||||
|
|
||||||
self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
|
self.kinematics = wpimath.SwerveDrive4Kinematics(
|
||||||
self.frontLeftLocation,
|
self.frontLeftLocation,
|
||||||
self.frontRightLocation,
|
self.frontRightLocation,
|
||||||
self.backLeftLocation,
|
self.backLeftLocation,
|
||||||
self.backRightLocation,
|
self.backRightLocation,
|
||||||
)
|
)
|
||||||
|
|
||||||
self.odometry = wpimath.kinematics.SwerveDrive4Odometry(
|
self.odometry = wpimath.SwerveDrive4Odometry(
|
||||||
self.kinematics,
|
self.kinematics,
|
||||||
self.gyro.getRotation2d(),
|
self.gyro.getRotation2d(),
|
||||||
(
|
(
|
||||||
@@ -75,7 +75,7 @@ class Drivetrain:
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
|
self.targetChassisVelocities = wpimath.ChassisVelocities()
|
||||||
|
|
||||||
self.gyro.reset()
|
self.gyro.reset()
|
||||||
|
|
||||||
@@ -95,27 +95,24 @@ class Drivetrain:
|
|||||||
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
||||||
:param periodSeconds: Time
|
:param periodSeconds: Time
|
||||||
"""
|
"""
|
||||||
swerveModuleStates = self.kinematics.toSwerveModuleStates(
|
chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot)
|
||||||
wpimath.kinematics.ChassisSpeeds.discretize(
|
if fieldRelative:
|
||||||
(
|
chassisVelocities = chassisVelocities.toRobotRelative(
|
||||||
wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
|
self.gyro.getRotation2d()
|
||||||
xSpeed, ySpeed, rot, self.gyro.getRotation2d()
|
|
||||||
)
|
|
||||||
if fieldRelative
|
|
||||||
else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
|
|
||||||
),
|
|
||||||
periodSeconds,
|
|
||||||
)
|
)
|
||||||
)
|
|
||||||
wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
|
|
||||||
swerveModuleStates, kMaxSpeed
|
|
||||||
)
|
|
||||||
self.frontLeft.setDesiredState(swerveModuleStates[0])
|
|
||||||
self.frontRight.setDesiredState(swerveModuleStates[1])
|
|
||||||
self.backLeft.setDesiredState(swerveModuleStates[2])
|
|
||||||
self.backRight.setDesiredState(swerveModuleStates[3])
|
|
||||||
|
|
||||||
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
|
chassisVelocities = chassisVelocities.discretize(periodSeconds)
|
||||||
|
swerveModuleStates = self.kinematics.desaturateWheelVelocities(
|
||||||
|
self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed
|
||||||
|
)
|
||||||
|
self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
|
||||||
|
self.frontRight.setDesiredVelocity(swerveModuleStates[1])
|
||||||
|
self.backLeft.setDesiredVelocity(swerveModuleStates[2])
|
||||||
|
self.backRight.setDesiredVelocity(swerveModuleStates[3])
|
||||||
|
|
||||||
|
self.targetChassisVelocities = self.kinematics.toChassisVelocities(
|
||||||
|
swerveModuleStates
|
||||||
|
)
|
||||||
|
|
||||||
def updateOdometry(self) -> None:
|
def updateOdometry(self) -> None:
|
||||||
"""Updates the field relative position of the robot."""
|
"""Updates the field relative position of the robot."""
|
||||||
@@ -129,26 +126,26 @@ class Drivetrain:
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
|
def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]:
|
||||||
return [
|
return [
|
||||||
self.frontLeft.getState(),
|
self.frontLeft.getVelocity(),
|
||||||
self.frontRight.getState(),
|
self.frontRight.getVelocity(),
|
||||||
self.backLeft.getState(),
|
self.backLeft.getVelocity(),
|
||||||
self.backRight.getState(),
|
self.backRight.getVelocity(),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
|
def getModulePoses(self) -> list[wpimath.Pose2d]:
|
||||||
p = self.odometry.getPose()
|
p = self.odometry.getPose()
|
||||||
flTrans = wpimath.geometry.Transform2d(
|
flTrans = wpimath.Transform2d(
|
||||||
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
frTrans = wpimath.geometry.Transform2d(
|
frTrans = wpimath.Transform2d(
|
||||||
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
blTrans = wpimath.geometry.Transform2d(
|
blTrans = wpimath.Transform2d(
|
||||||
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
brTrans = wpimath.geometry.Transform2d(
|
brTrans = wpimath.Transform2d(
|
||||||
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
return [
|
return [
|
||||||
@@ -158,8 +155,8 @@ class Drivetrain:
|
|||||||
p.transformBy(brTrans),
|
p.transformBy(brTrans),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
|
def getChassisVelocities(self) -> wpimath.ChassisVelocities:
|
||||||
return self.kinematics.toChassisSpeeds(self.getModuleStates())
|
return self.kinematics.toChassisVelocities(self.getModuleVelocities())
|
||||||
|
|
||||||
def log(self):
|
def log(self):
|
||||||
table = "Drive/"
|
table = "Drive/"
|
||||||
@@ -169,7 +166,7 @@ class Drivetrain:
|
|||||||
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
||||||
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
||||||
|
|
||||||
chassisSpeeds = self.getChassisSpeeds()
|
chassisSpeeds = self.getChassisVelocities()
|
||||||
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
||||||
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -177,13 +174,13 @@ class Drivetrain:
|
|||||||
)
|
)
|
||||||
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VX", self.targetChassisSpeeds.vx
|
table + "Target VX", self.targetChassisVelocities.vx
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VY", self.targetChassisSpeeds.vy
|
table + "Target VY", self.targetChassisVelocities.vy
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
|
table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps
|
||||||
)
|
)
|
||||||
|
|
||||||
self.frontLeft.log()
|
self.frontLeft.log()
|
||||||
@@ -199,5 +196,7 @@ class Drivetrain:
|
|||||||
self.frontRight.simulationPeriodic()
|
self.frontRight.simulationPeriodic()
|
||||||
self.backLeft.simulationPeriodic()
|
self.backLeft.simulationPeriodic()
|
||||||
self.backRight.simulationPeriodic()
|
self.backRight.simulationPeriodic()
|
||||||
self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
|
rate = -1.0 * self.getChassisVelocities().omega_dps
|
||||||
self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
|
self.simGyro.setRate(rate)
|
||||||
|
self._yaw += rate * 0.02
|
||||||
|
self.simGyro.setYaw(self._yaw)
|
||||||
|
|||||||
@@ -39,9 +39,11 @@ TAG_7_MOUNT_HEIGHT_m = 1.435 # From the 2024 game manual
|
|||||||
|
|
||||||
|
|
||||||
class MyRobot(wpilib.TimedRobot):
|
class MyRobot(wpilib.TimedRobot):
|
||||||
def robotInit(self) -> None:
|
def __init__(self) -> None:
|
||||||
"""Robot initialization function"""
|
"""Robot initialization function"""
|
||||||
self.controller = wpilib.XboxController(0)
|
super().__init__()
|
||||||
|
|
||||||
|
self.controller = wpilib.NiDsXboxController(0)
|
||||||
self.swerve = drivetrain.Drivetrain()
|
self.swerve = drivetrain.Drivetrain()
|
||||||
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
||||||
|
|
||||||
|
|||||||
@@ -26,11 +26,7 @@ import math
|
|||||||
|
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.controller
|
import wpimath
|
||||||
import wpimath.filter
|
|
||||||
import wpimath.geometry
|
|
||||||
import wpimath.kinematics
|
|
||||||
import wpimath.trajectory
|
|
||||||
import wpimath.units
|
import wpimath.units
|
||||||
|
|
||||||
kWheelRadius = 0.0508
|
kWheelRadius = 0.0508
|
||||||
@@ -60,7 +56,7 @@ class SwerveModule:
|
|||||||
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
||||||
"""
|
"""
|
||||||
self.moduleNumber = moduleNumber
|
self.moduleNumber = moduleNumber
|
||||||
self.desiredState = wpimath.kinematics.SwerveModuleState()
|
self.desiredVelocity = wpimath.SwerveModuleVelocity()
|
||||||
|
|
||||||
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
||||||
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
||||||
@@ -71,14 +67,14 @@ class SwerveModule:
|
|||||||
)
|
)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
|
self.drivePIDController = wpimath.PIDController(10, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
|
self.turningPIDController = wpimath.PIDController(30, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
|
self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
|
||||||
self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
|
self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7)
|
||||||
|
|
||||||
# Set the distance per pulse for the drive encoder. We can simply use the
|
# Set the distance per pulse for the drive encoder. We can simply use the
|
||||||
# distance traveled for one rotation of the wheel divided by the encoder
|
# distance traveled for one rotation of the wheel divided by the encoder
|
||||||
@@ -99,66 +95,60 @@ class SwerveModule:
|
|||||||
# Simulation Support
|
# Simulation Support
|
||||||
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
||||||
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
||||||
self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
|
self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel)
|
||||||
self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
|
self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel)
|
||||||
self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02)
|
||||||
0.1, 0.02
|
self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02)
|
||||||
)
|
|
||||||
self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
|
||||||
0.0001, 0.02
|
|
||||||
)
|
|
||||||
self.simTurningEncoderPos = 0
|
self.simTurningEncoderPos = 0
|
||||||
self.simDrivingEncoderPos = 0
|
self.simDrivingEncoderPos = 0
|
||||||
|
|
||||||
def getState(self) -> wpimath.kinematics.SwerveModuleState:
|
def getVelocity(self) -> wpimath.SwerveModuleVelocity:
|
||||||
"""Returns the current state of the module.
|
"""Returns the current state of the module.
|
||||||
|
|
||||||
:returns: The current state of the module.
|
:returns: The current state of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModuleState(
|
return wpimath.SwerveModuleVelocity(
|
||||||
self.driveEncoder.getRate(),
|
self.driveEncoder.getRate(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
|
def getPosition(self) -> wpimath.SwerveModulePosition:
|
||||||
"""Returns the current position of the module.
|
"""Returns the current position of the module.
|
||||||
|
|
||||||
:returns: The current position of the module.
|
:returns: The current position of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModulePosition(
|
return wpimath.SwerveModulePosition(
|
||||||
self.driveEncoder.getDistance(),
|
self.driveEncoder.getDistance(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def setDesiredState(
|
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
|
||||||
self, desiredState: wpimath.kinematics.SwerveModuleState
|
|
||||||
) -> None:
|
|
||||||
"""Sets the desired state for the module.
|
"""Sets the desired state for the module.
|
||||||
|
|
||||||
:param desiredState: Desired state with speed and angle.
|
:param desiredVelocity: Desired state with speed and angle.
|
||||||
"""
|
"""
|
||||||
self.desiredState = desiredState
|
self.desiredVelocity = desiredVelocity
|
||||||
|
|
||||||
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
# Optimize the reference state to avoid spinning further than 90 degrees
|
# Optimize the reference state to avoid spinning further than 90 degrees
|
||||||
desiredState.optimize(encoderRotation)
|
desiredVelocity.optimize(encoderRotation)
|
||||||
|
|
||||||
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||||||
# direction of travel that can occur when modules change directions. This results in smoother
|
# direction of travel that can occur when modules change directions. This results in smoother
|
||||||
# driving.
|
# driving.
|
||||||
desiredState.speed *= (desiredState.angle - encoderRotation).cos()
|
desiredVelocity.velocity *= (desiredVelocity.angle - encoderRotation).cos()
|
||||||
|
|
||||||
# Calculate the drive output from the drive PID controller.
|
# Calculate the drive output from the drive PID controller.
|
||||||
driveOutput = self.drivePIDController.calculate(
|
driveOutput = self.drivePIDController.calculate(
|
||||||
self.driveEncoder.getRate(), desiredState.speed
|
self.driveEncoder.getRate(), desiredVelocity.velocity
|
||||||
)
|
)
|
||||||
|
|
||||||
driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
|
driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
|
||||||
|
|
||||||
# Calculate the turning motor output from the turning PID controller.
|
# Calculate the turning motor output from the turning PID controller.
|
||||||
turnOutput = self.turningPIDController.calculate(
|
turnOutput = self.turningPIDController.calculate(
|
||||||
self.turningEncoder.getDistance(), desiredState.angle.radians()
|
self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
|
||||||
)
|
)
|
||||||
|
|
||||||
turnFeedforward = self.turnFeedforward.calculate(
|
turnFeedforward = self.turnFeedforward.calculate(
|
||||||
@@ -168,11 +158,11 @@ class SwerveModule:
|
|||||||
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
||||||
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
||||||
|
|
||||||
def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
|
def getAbsoluteHeading(self) -> wpimath.Rotation2d:
|
||||||
return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
return wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
def log(self) -> None:
|
def log(self) -> None:
|
||||||
state = self.getState()
|
state = self.getVelocity()
|
||||||
|
|
||||||
table = "Module " + str(self.moduleNumber) + "/"
|
table = "Module " + str(self.moduleNumber) + "/"
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -183,15 +173,17 @@ class SwerveModule:
|
|||||||
table + "Steer Target Degrees",
|
table + "Steer Target Degrees",
|
||||||
math.degrees(self.turningPIDController.getSetpoint()),
|
math.degrees(self.turningPIDController.getSetpoint()),
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Velocity Target Feet", self.desiredState.speed_fps
|
table + "Drive Velocity Feet", state.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Voltage", self.driveMotor.get() * 12.0
|
table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Steer Voltage", self.turningMotor.get() * 12.0
|
table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0
|
||||||
|
)
|
||||||
|
wpilib.SmartDashboard.putNumber(
|
||||||
|
table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0
|
||||||
)
|
)
|
||||||
|
|
||||||
def simulationPeriodic(self) -> None:
|
def simulationPeriodic(self) -> None:
|
||||||
|
|||||||
@@ -28,8 +28,7 @@ import math
|
|||||||
import swervemodule
|
import swervemodule
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.geometry
|
import wpimath
|
||||||
import wpimath.kinematics
|
|
||||||
|
|
||||||
kMaxSpeed = 3.0 # 3 meters per second
|
kMaxSpeed = 3.0 # 3 meters per second
|
||||||
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
||||||
@@ -41,10 +40,10 @@ class Drivetrain:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
|
self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
|
||||||
self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
|
self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
|
||||||
self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
|
self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
|
||||||
self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
|
self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)
|
||||||
|
|
||||||
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
||||||
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
||||||
@@ -55,16 +54,17 @@ class Drivetrain:
|
|||||||
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
||||||
|
|
||||||
self.gyro = wpilib.AnalogGyro(0)
|
self.gyro = wpilib.AnalogGyro(0)
|
||||||
self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
|
self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro)
|
||||||
|
self._yaw = 0.0
|
||||||
|
|
||||||
self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
|
self.kinematics = wpimath.SwerveDrive4Kinematics(
|
||||||
self.frontLeftLocation,
|
self.frontLeftLocation,
|
||||||
self.frontRightLocation,
|
self.frontRightLocation,
|
||||||
self.backLeftLocation,
|
self.backLeftLocation,
|
||||||
self.backRightLocation,
|
self.backRightLocation,
|
||||||
)
|
)
|
||||||
|
|
||||||
self.odometry = wpimath.kinematics.SwerveDrive4Odometry(
|
self.odometry = wpimath.SwerveDrive4Odometry(
|
||||||
self.kinematics,
|
self.kinematics,
|
||||||
self.gyro.getRotation2d(),
|
self.gyro.getRotation2d(),
|
||||||
(
|
(
|
||||||
@@ -75,7 +75,7 @@ class Drivetrain:
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
|
self.targetChassisVelocities = wpimath.ChassisVelocities()
|
||||||
|
|
||||||
self.gyro.reset()
|
self.gyro.reset()
|
||||||
|
|
||||||
@@ -95,27 +95,24 @@ class Drivetrain:
|
|||||||
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
||||||
:param periodSeconds: Time
|
:param periodSeconds: Time
|
||||||
"""
|
"""
|
||||||
swerveModuleStates = self.kinematics.toSwerveModuleStates(
|
chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot)
|
||||||
wpimath.kinematics.ChassisSpeeds.discretize(
|
if fieldRelative:
|
||||||
(
|
chassisVelocities = chassisVelocities.toRobotRelative(
|
||||||
wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
|
self.gyro.getRotation2d()
|
||||||
xSpeed, ySpeed, rot, self.gyro.getRotation2d()
|
|
||||||
)
|
|
||||||
if fieldRelative
|
|
||||||
else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
|
|
||||||
),
|
|
||||||
periodSeconds,
|
|
||||||
)
|
)
|
||||||
)
|
|
||||||
wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
|
|
||||||
swerveModuleStates, kMaxSpeed
|
|
||||||
)
|
|
||||||
self.frontLeft.setDesiredState(swerveModuleStates[0])
|
|
||||||
self.frontRight.setDesiredState(swerveModuleStates[1])
|
|
||||||
self.backLeft.setDesiredState(swerveModuleStates[2])
|
|
||||||
self.backRight.setDesiredState(swerveModuleStates[3])
|
|
||||||
|
|
||||||
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
|
chassisVelocities = chassisVelocities.discretize(periodSeconds)
|
||||||
|
swerveModuleStates = self.kinematics.desaturateWheelVelocities(
|
||||||
|
self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed
|
||||||
|
)
|
||||||
|
self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
|
||||||
|
self.frontRight.setDesiredVelocity(swerveModuleStates[1])
|
||||||
|
self.backLeft.setDesiredVelocity(swerveModuleStates[2])
|
||||||
|
self.backRight.setDesiredVelocity(swerveModuleStates[3])
|
||||||
|
|
||||||
|
self.targetChassisVelocities = self.kinematics.toChassisVelocities(
|
||||||
|
swerveModuleStates
|
||||||
|
)
|
||||||
|
|
||||||
def updateOdometry(self) -> None:
|
def updateOdometry(self) -> None:
|
||||||
"""Updates the field relative position of the robot."""
|
"""Updates the field relative position of the robot."""
|
||||||
@@ -129,26 +126,26 @@ class Drivetrain:
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
|
def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]:
|
||||||
return [
|
return [
|
||||||
self.frontLeft.getState(),
|
self.frontLeft.getVelocity(),
|
||||||
self.frontRight.getState(),
|
self.frontRight.getVelocity(),
|
||||||
self.backLeft.getState(),
|
self.backLeft.getVelocity(),
|
||||||
self.backRight.getState(),
|
self.backRight.getVelocity(),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
|
def getModulePoses(self) -> list[wpimath.Pose2d]:
|
||||||
p = self.odometry.getPose()
|
p = self.odometry.getPose()
|
||||||
flTrans = wpimath.geometry.Transform2d(
|
flTrans = wpimath.Transform2d(
|
||||||
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
frTrans = wpimath.geometry.Transform2d(
|
frTrans = wpimath.Transform2d(
|
||||||
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
blTrans = wpimath.geometry.Transform2d(
|
blTrans = wpimath.Transform2d(
|
||||||
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
brTrans = wpimath.geometry.Transform2d(
|
brTrans = wpimath.Transform2d(
|
||||||
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
return [
|
return [
|
||||||
@@ -158,8 +155,8 @@ class Drivetrain:
|
|||||||
p.transformBy(brTrans),
|
p.transformBy(brTrans),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
|
def getChassisVelocities(self) -> wpimath.ChassisVelocities:
|
||||||
return self.kinematics.toChassisSpeeds(self.getModuleStates())
|
return self.kinematics.toChassisVelocities(self.getModuleVelocities())
|
||||||
|
|
||||||
def log(self):
|
def log(self):
|
||||||
table = "Drive/"
|
table = "Drive/"
|
||||||
@@ -169,7 +166,7 @@ class Drivetrain:
|
|||||||
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
||||||
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
||||||
|
|
||||||
chassisSpeeds = self.getChassisSpeeds()
|
chassisSpeeds = self.getChassisVelocities()
|
||||||
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
||||||
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -177,13 +174,13 @@ class Drivetrain:
|
|||||||
)
|
)
|
||||||
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VX", self.targetChassisSpeeds.vx
|
table + "Target VX", self.targetChassisVelocities.vx
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VY", self.targetChassisSpeeds.vy
|
table + "Target VY", self.targetChassisVelocities.vy
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
|
table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps
|
||||||
)
|
)
|
||||||
|
|
||||||
self.frontLeft.log()
|
self.frontLeft.log()
|
||||||
@@ -199,5 +196,7 @@ class Drivetrain:
|
|||||||
self.frontRight.simulationPeriodic()
|
self.frontRight.simulationPeriodic()
|
||||||
self.backLeft.simulationPeriodic()
|
self.backLeft.simulationPeriodic()
|
||||||
self.backRight.simulationPeriodic()
|
self.backRight.simulationPeriodic()
|
||||||
self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
|
rate = -1.0 * self.getChassisVelocities().omega_dps
|
||||||
self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
|
self.simGyro.setRate(rate)
|
||||||
|
self._yaw += rate * 0.02
|
||||||
|
self.simGyro.setYaw(self._yaw)
|
||||||
|
|||||||
@@ -32,9 +32,11 @@ VISION_TURN_kP = 0.01
|
|||||||
|
|
||||||
|
|
||||||
class MyRobot(wpilib.TimedRobot):
|
class MyRobot(wpilib.TimedRobot):
|
||||||
def robotInit(self) -> None:
|
def __init__(self) -> None:
|
||||||
"""Robot initialization function"""
|
"""Robot initialization function"""
|
||||||
self.controller = wpilib.XboxController(0)
|
super().__init__()
|
||||||
|
|
||||||
|
self.controller = wpilib.NiDsXboxController(0)
|
||||||
self.swerve = drivetrain.Drivetrain()
|
self.swerve = drivetrain.Drivetrain()
|
||||||
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
||||||
|
|
||||||
|
|||||||
@@ -26,11 +26,7 @@ import math
|
|||||||
|
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.controller
|
import wpimath
|
||||||
import wpimath.filter
|
|
||||||
import wpimath.geometry
|
|
||||||
import wpimath.kinematics
|
|
||||||
import wpimath.trajectory
|
|
||||||
import wpimath.units
|
import wpimath.units
|
||||||
|
|
||||||
kWheelRadius = 0.0508
|
kWheelRadius = 0.0508
|
||||||
@@ -60,7 +56,7 @@ class SwerveModule:
|
|||||||
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
||||||
"""
|
"""
|
||||||
self.moduleNumber = moduleNumber
|
self.moduleNumber = moduleNumber
|
||||||
self.desiredState = wpimath.kinematics.SwerveModuleState()
|
self.desiredVelocity = wpimath.SwerveModuleVelocity()
|
||||||
|
|
||||||
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
||||||
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
||||||
@@ -71,14 +67,14 @@ class SwerveModule:
|
|||||||
)
|
)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
|
self.drivePIDController = wpimath.PIDController(10, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
|
self.turningPIDController = wpimath.PIDController(30, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
|
self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
|
||||||
self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
|
self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7)
|
||||||
|
|
||||||
# Set the distance per pulse for the drive encoder. We can simply use the
|
# Set the distance per pulse for the drive encoder. We can simply use the
|
||||||
# distance traveled for one rotation of the wheel divided by the encoder
|
# distance traveled for one rotation of the wheel divided by the encoder
|
||||||
@@ -99,66 +95,60 @@ class SwerveModule:
|
|||||||
# Simulation Support
|
# Simulation Support
|
||||||
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
||||||
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
||||||
self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
|
self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel)
|
||||||
self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
|
self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel)
|
||||||
self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02)
|
||||||
0.1, 0.02
|
self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02)
|
||||||
)
|
|
||||||
self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
|
||||||
0.0001, 0.02
|
|
||||||
)
|
|
||||||
self.simTurningEncoderPos = 0
|
self.simTurningEncoderPos = 0
|
||||||
self.simDrivingEncoderPos = 0
|
self.simDrivingEncoderPos = 0
|
||||||
|
|
||||||
def getState(self) -> wpimath.kinematics.SwerveModuleState:
|
def getVelocity(self) -> wpimath.SwerveModuleVelocity:
|
||||||
"""Returns the current state of the module.
|
"""Returns the current state of the module.
|
||||||
|
|
||||||
:returns: The current state of the module.
|
:returns: The current state of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModuleState(
|
return wpimath.SwerveModuleVelocity(
|
||||||
self.driveEncoder.getRate(),
|
self.driveEncoder.getRate(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
|
def getPosition(self) -> wpimath.SwerveModulePosition:
|
||||||
"""Returns the current position of the module.
|
"""Returns the current position of the module.
|
||||||
|
|
||||||
:returns: The current position of the module.
|
:returns: The current position of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModulePosition(
|
return wpimath.SwerveModulePosition(
|
||||||
self.driveEncoder.getDistance(),
|
self.driveEncoder.getDistance(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def setDesiredState(
|
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
|
||||||
self, desiredState: wpimath.kinematics.SwerveModuleState
|
|
||||||
) -> None:
|
|
||||||
"""Sets the desired state for the module.
|
"""Sets the desired state for the module.
|
||||||
|
|
||||||
:param desiredState: Desired state with speed and angle.
|
:param desiredVelocity: Desired state with speed and angle.
|
||||||
"""
|
"""
|
||||||
self.desiredState = desiredState
|
self.desiredVelocity = desiredVelocity
|
||||||
|
|
||||||
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
# Optimize the reference state to avoid spinning further than 90 degrees
|
# Optimize the reference state to avoid spinning further than 90 degrees
|
||||||
desiredState.optimize(encoderRotation)
|
desiredVelocity.optimize(encoderRotation)
|
||||||
|
|
||||||
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||||||
# direction of travel that can occur when modules change directions. This results in smoother
|
# direction of travel that can occur when modules change directions. This results in smoother
|
||||||
# driving.
|
# driving.
|
||||||
desiredState.speed *= (desiredState.angle - encoderRotation).cos()
|
desiredVelocity.velocity *= (desiredVelocity.angle - encoderRotation).cos()
|
||||||
|
|
||||||
# Calculate the drive output from the drive PID controller.
|
# Calculate the drive output from the drive PID controller.
|
||||||
driveOutput = self.drivePIDController.calculate(
|
driveOutput = self.drivePIDController.calculate(
|
||||||
self.driveEncoder.getRate(), desiredState.speed
|
self.driveEncoder.getRate(), desiredVelocity.velocity
|
||||||
)
|
)
|
||||||
|
|
||||||
driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
|
driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
|
||||||
|
|
||||||
# Calculate the turning motor output from the turning PID controller.
|
# Calculate the turning motor output from the turning PID controller.
|
||||||
turnOutput = self.turningPIDController.calculate(
|
turnOutput = self.turningPIDController.calculate(
|
||||||
self.turningEncoder.getDistance(), desiredState.angle.radians()
|
self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
|
||||||
)
|
)
|
||||||
|
|
||||||
turnFeedforward = self.turnFeedforward.calculate(
|
turnFeedforward = self.turnFeedforward.calculate(
|
||||||
@@ -168,11 +158,11 @@ class SwerveModule:
|
|||||||
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
||||||
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
||||||
|
|
||||||
def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
|
def getAbsoluteHeading(self) -> wpimath.Rotation2d:
|
||||||
return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
return wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
def log(self) -> None:
|
def log(self) -> None:
|
||||||
state = self.getState()
|
state = self.getVelocity()
|
||||||
|
|
||||||
table = "Module " + str(self.moduleNumber) + "/"
|
table = "Module " + str(self.moduleNumber) + "/"
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -183,15 +173,17 @@ class SwerveModule:
|
|||||||
table + "Steer Target Degrees",
|
table + "Steer Target Degrees",
|
||||||
math.degrees(self.turningPIDController.getSetpoint()),
|
math.degrees(self.turningPIDController.getSetpoint()),
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Velocity Target Feet", self.desiredState.speed_fps
|
table + "Drive Velocity Feet", state.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Voltage", self.driveMotor.get() * 12.0
|
table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Steer Voltage", self.turningMotor.get() * 12.0
|
table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0
|
||||||
|
)
|
||||||
|
wpilib.SmartDashboard.putNumber(
|
||||||
|
table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0
|
||||||
)
|
)
|
||||||
|
|
||||||
def simulationPeriodic(self) -> None:
|
def simulationPeriodic(self) -> None:
|
||||||
|
|||||||
@@ -28,16 +28,14 @@ import math
|
|||||||
import swervemodule
|
import swervemodule
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.estimator
|
import wpimath
|
||||||
import wpimath.geometry
|
|
||||||
import wpimath.kinematics
|
|
||||||
|
|
||||||
kMaxSpeed = 3.0 # 3 meters per second
|
kMaxSpeed = 3.0 # 3 meters per second
|
||||||
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
kMaxAngularSpeed = math.pi # 1/2 rotation per second
|
||||||
|
|
||||||
kInitialPose = wpimath.geometry.Pose2d(
|
kInitialPose = wpimath.Pose2d(
|
||||||
wpimath.geometry.Translation2d(1.0, 1.0),
|
wpimath.Translation2d(1.0, 1.0),
|
||||||
wpimath.geometry.Rotation2d.fromDegrees(0.0),
|
wpimath.Rotation2d.fromDegrees(0.0),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -47,10 +45,10 @@ class Drivetrain:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
|
self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
|
||||||
self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
|
self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
|
||||||
self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
|
self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
|
||||||
self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
|
self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)
|
||||||
|
|
||||||
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
|
||||||
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
|
||||||
@@ -61,16 +59,17 @@ class Drivetrain:
|
|||||||
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
|
||||||
|
|
||||||
self.gyro = wpilib.AnalogGyro(0)
|
self.gyro = wpilib.AnalogGyro(0)
|
||||||
self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
|
self.simGyro = wpilib.simulation.OnboardIMUSim(self.gyro)
|
||||||
|
self._yaw = 0.0
|
||||||
|
|
||||||
self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
|
self.kinematics = wpimath.SwerveDrive4Kinematics(
|
||||||
self.frontLeftLocation,
|
self.frontLeftLocation,
|
||||||
self.frontRightLocation,
|
self.frontRightLocation,
|
||||||
self.backLeftLocation,
|
self.backLeftLocation,
|
||||||
self.backRightLocation,
|
self.backRightLocation,
|
||||||
)
|
)
|
||||||
|
|
||||||
self.poseEst = wpimath.estimator.SwerveDrive4PoseEstimator(
|
self.poseEst = wpimath.SwerveDrive4PoseEstimator(
|
||||||
self.kinematics,
|
self.kinematics,
|
||||||
self.gyro.getRotation2d(),
|
self.gyro.getRotation2d(),
|
||||||
(
|
(
|
||||||
@@ -79,10 +78,9 @@ class Drivetrain:
|
|||||||
self.backLeft.getPosition(),
|
self.backLeft.getPosition(),
|
||||||
self.backRight.getPosition(),
|
self.backRight.getPosition(),
|
||||||
),
|
),
|
||||||
kInitialPose,
|
|
||||||
)
|
)
|
||||||
|
|
||||||
self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
|
self.targetChassisVelocities = wpimath.ChassisVelocities()
|
||||||
|
|
||||||
self.gyro.reset()
|
self.gyro.reset()
|
||||||
|
|
||||||
@@ -102,31 +100,28 @@ class Drivetrain:
|
|||||||
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
|
||||||
:param periodSeconds: Time
|
:param periodSeconds: Time
|
||||||
"""
|
"""
|
||||||
swerveModuleStates = self.kinematics.toSwerveModuleStates(
|
chassisVelocities = wpimath.ChassisVelocities(xSpeed, ySpeed, rot)
|
||||||
wpimath.kinematics.ChassisSpeeds.discretize(
|
if fieldRelative:
|
||||||
(
|
chassisVelocities = chassisVelocities.toRobotRelative(
|
||||||
wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
|
self.gyro.getRotation2d()
|
||||||
xSpeed, ySpeed, rot, self.gyro.getRotation2d()
|
|
||||||
)
|
|
||||||
if fieldRelative
|
|
||||||
else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
|
|
||||||
),
|
|
||||||
periodSeconds,
|
|
||||||
)
|
)
|
||||||
)
|
|
||||||
wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
|
|
||||||
swerveModuleStates, kMaxSpeed
|
|
||||||
)
|
|
||||||
self.frontLeft.setDesiredState(swerveModuleStates[0])
|
|
||||||
self.frontRight.setDesiredState(swerveModuleStates[1])
|
|
||||||
self.backLeft.setDesiredState(swerveModuleStates[2])
|
|
||||||
self.backRight.setDesiredState(swerveModuleStates[3])
|
|
||||||
|
|
||||||
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
|
chassisVelocities = chassisVelocities.discretize(periodSeconds)
|
||||||
|
swerveModuleStates = self.kinematics.desaturateWheelVelocities(
|
||||||
|
self.kinematics.toSwerveModuleVelocities(chassisVelocities), kMaxSpeed
|
||||||
|
)
|
||||||
|
self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
|
||||||
|
self.frontRight.setDesiredVelocity(swerveModuleStates[1])
|
||||||
|
self.backLeft.setDesiredVelocity(swerveModuleStates[2])
|
||||||
|
self.backRight.setDesiredVelocity(swerveModuleStates[3])
|
||||||
|
|
||||||
|
self.targetChassisVelocities = self.kinematics.toChassisVelocities(
|
||||||
|
swerveModuleStates
|
||||||
|
)
|
||||||
|
|
||||||
def updateOdometry(self) -> None:
|
def updateOdometry(self) -> None:
|
||||||
"""Updates the field relative position of the robot."""
|
"""Updates the field relative position of the robot."""
|
||||||
self.poseEst.update(
|
self.odometry.update(
|
||||||
self.gyro.getRotation2d(),
|
self.gyro.getRotation2d(),
|
||||||
(
|
(
|
||||||
self.frontLeft.getPosition(),
|
self.frontLeft.getPosition(),
|
||||||
@@ -136,9 +131,7 @@ class Drivetrain:
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
def addVisionPoseEstimate(
|
def addVisionPoseEstimate(self, pose: wpimath.Pose3d, timestamp: float) -> None:
|
||||||
self, pose: wpimath.geometry.Pose3d, timestamp: float
|
|
||||||
) -> None:
|
|
||||||
self.poseEst.addVisionMeasurement(pose, timestamp)
|
self.poseEst.addVisionMeasurement(pose, timestamp)
|
||||||
|
|
||||||
def resetPose(self) -> None:
|
def resetPose(self) -> None:
|
||||||
@@ -153,26 +146,26 @@ class Drivetrain:
|
|||||||
kInitialPose,
|
kInitialPose,
|
||||||
)
|
)
|
||||||
|
|
||||||
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
|
def getModuleVelocities(self) -> list[wpimath.SwerveModuleVelocity]:
|
||||||
return [
|
return [
|
||||||
self.frontLeft.getState(),
|
self.frontLeft.getVelocity(),
|
||||||
self.frontRight.getState(),
|
self.frontRight.getVelocity(),
|
||||||
self.backLeft.getState(),
|
self.backLeft.getVelocity(),
|
||||||
self.backRight.getState(),
|
self.backRight.getVelocity(),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
|
def getModulePoses(self) -> list[wpimath.Pose2d]:
|
||||||
p = self.poseEst.getEstimatedPosition()
|
p = self.poseEst.getEstimatedPosition()
|
||||||
flTrans = wpimath.geometry.Transform2d(
|
flTrans = wpimath.Transform2d(
|
||||||
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
frTrans = wpimath.geometry.Transform2d(
|
frTrans = wpimath.Transform2d(
|
||||||
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
self.frontRightLocation, self.frontRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
blTrans = wpimath.geometry.Transform2d(
|
blTrans = wpimath.Transform2d(
|
||||||
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
self.backLeftLocation, self.backLeft.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
brTrans = wpimath.geometry.Transform2d(
|
brTrans = wpimath.Transform2d(
|
||||||
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
self.backRightLocation, self.backRight.getAbsoluteHeading()
|
||||||
)
|
)
|
||||||
return [
|
return [
|
||||||
@@ -182,18 +175,18 @@ class Drivetrain:
|
|||||||
p.transformBy(brTrans),
|
p.transformBy(brTrans),
|
||||||
]
|
]
|
||||||
|
|
||||||
def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
|
def getChassisVelocities(self) -> wpimath.ChassisVelocities:
|
||||||
return self.kinematics.toChassisSpeeds(self.getModuleStates())
|
return self.kinematics.toChassisVelocities(self.getModuleVelocities())
|
||||||
|
|
||||||
def log(self):
|
def log(self):
|
||||||
table = "Drive/"
|
table = "Drive/"
|
||||||
|
|
||||||
pose = self.poseEst.getEstimatedPosition()
|
pose = self.odometry.getPose()
|
||||||
wpilib.SmartDashboard.putNumber(table + "X", pose.X())
|
wpilib.SmartDashboard.putNumber(table + "X", pose.X())
|
||||||
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
|
||||||
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
|
||||||
|
|
||||||
chassisSpeeds = self.getChassisSpeeds()
|
chassisSpeeds = self.getChassisVelocities()
|
||||||
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
|
||||||
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -201,13 +194,13 @@ class Drivetrain:
|
|||||||
)
|
)
|
||||||
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VX", self.targetChassisSpeeds.vx
|
table + "Target VX", self.targetChassisVelocities.vx
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target VY", self.targetChassisSpeeds.vy
|
table + "Target VY", self.targetChassisVelocities.vy
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
|
table + "Target Omega Degrees", self.targetChassisVelocities.omega_dps
|
||||||
)
|
)
|
||||||
|
|
||||||
self.frontLeft.log()
|
self.frontLeft.log()
|
||||||
@@ -215,7 +208,7 @@ class Drivetrain:
|
|||||||
self.backLeft.log()
|
self.backLeft.log()
|
||||||
self.backRight.log()
|
self.backRight.log()
|
||||||
|
|
||||||
self.debugField.getRobotObject().setPose(self.poseEst.getEstimatedPosition())
|
self.debugField.getRobotObject().setPose(self.odometry.getPose())
|
||||||
self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses())
|
self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses())
|
||||||
|
|
||||||
def simulationPeriodic(self):
|
def simulationPeriodic(self):
|
||||||
@@ -223,5 +216,7 @@ class Drivetrain:
|
|||||||
self.frontRight.simulationPeriodic()
|
self.frontRight.simulationPeriodic()
|
||||||
self.backLeft.simulationPeriodic()
|
self.backLeft.simulationPeriodic()
|
||||||
self.backRight.simulationPeriodic()
|
self.backRight.simulationPeriodic()
|
||||||
self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
|
rate = -1.0 * self.getChassisVelocities().omega_dps
|
||||||
self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
|
self.simGyro.setRate(rate)
|
||||||
|
self._yaw += rate * 0.02
|
||||||
|
self.simGyro.setYaw(self._yaw)
|
||||||
|
|||||||
@@ -26,20 +26,22 @@
|
|||||||
|
|
||||||
import drivetrain
|
import drivetrain
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpimath.geometry
|
import wpimath
|
||||||
from photonlibpy import PhotonCamera, PhotonPoseEstimator
|
from photonlibpy import PhotonCamera, PhotonPoseEstimator
|
||||||
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
|
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
|
||||||
|
|
||||||
kRobotToCam = wpimath.geometry.Transform3d(
|
kRobotToCam = wpimath.Transform3d(
|
||||||
wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
|
wpimath.Translation3d(0.5, 0.0, 0.5),
|
||||||
wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
|
wpimath.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
class MyRobot(wpilib.TimedRobot):
|
class MyRobot(wpilib.TimedRobot):
|
||||||
def robotInit(self) -> None:
|
def __init__(self) -> None:
|
||||||
"""Robot initialization function"""
|
"""Robot initialization function"""
|
||||||
self.controller = wpilib.XboxController(0)
|
super().__init__()
|
||||||
|
|
||||||
|
self.controller = wpilib.NiDsXboxController(0)
|
||||||
self.swerve = drivetrain.Drivetrain()
|
self.swerve = drivetrain.Drivetrain()
|
||||||
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
||||||
self.camPoseEst = PhotonPoseEstimator(
|
self.camPoseEst = PhotonPoseEstimator(
|
||||||
|
|||||||
@@ -1,4 +1,4 @@
|
|||||||
###################################################################################
|
Speeds ###################################################################################
|
||||||
# MIT License
|
# MIT License
|
||||||
#
|
#
|
||||||
# Copyright (c) PhotonVision
|
# Copyright (c) PhotonVision
|
||||||
@@ -26,11 +26,7 @@ import math
|
|||||||
|
|
||||||
import wpilib
|
import wpilib
|
||||||
import wpilib.simulation
|
import wpilib.simulation
|
||||||
import wpimath.controller
|
import wpimath
|
||||||
import wpimath.filter
|
|
||||||
import wpimath.geometry
|
|
||||||
import wpimath.kinematics
|
|
||||||
import wpimath.trajectory
|
|
||||||
import wpimath.units
|
import wpimath.units
|
||||||
|
|
||||||
kWheelRadius = 0.0508
|
kWheelRadius = 0.0508
|
||||||
@@ -60,7 +56,7 @@ class SwerveModule:
|
|||||||
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
:param turningEncoderChannelB: DIO input for the turning encoder channel B
|
||||||
"""
|
"""
|
||||||
self.moduleNumber = moduleNumber
|
self.moduleNumber = moduleNumber
|
||||||
self.desiredState = wpimath.kinematics.SwerveModuleState()
|
self.desiredVelocity = wpimath.SwerveModuleVelocity()
|
||||||
|
|
||||||
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
|
||||||
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
|
||||||
@@ -71,13 +67,14 @@ class SwerveModule:
|
|||||||
)
|
)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
|
self.drivePIDController = wpimath.PIDController(10, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
|
self.turningPIDController = wpimath.PIDController(30, 0, 0)
|
||||||
|
|
||||||
# Gains are for example purposes only - must be determined for your own robot!
|
# Gains are for example purposes only - must be determined for your own robot!
|
||||||
self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
|
self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
|
||||||
|
self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.7)
|
||||||
|
|
||||||
# Set the distance per pulse for the drive encoder. We can simply use the
|
# Set the distance per pulse for the drive encoder. We can simply use the
|
||||||
# distance traveled for one rotation of the wheel divided by the encoder
|
# distance traveled for one rotation of the wheel divided by the encoder
|
||||||
@@ -98,76 +95,74 @@ class SwerveModule:
|
|||||||
# Simulation Support
|
# Simulation Support
|
||||||
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
|
||||||
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
|
||||||
self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
|
self.simDrivingMotor = wpilib.simulation.PWMSim(driveMotorChannel)
|
||||||
self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
|
self.simTurningMotor = wpilib.simulation.PWMSim(turningMotorChannel)
|
||||||
self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
self.simDrivingMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.1, 0.02)
|
||||||
0.1, 0.02
|
self.simTurningMotorFilter = wpimath.LinearFilter.singlePoleIIR(0.0001, 0.02)
|
||||||
)
|
|
||||||
self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
|
|
||||||
0.0001, 0.02
|
|
||||||
)
|
|
||||||
self.simTurningEncoderPos = 0
|
self.simTurningEncoderPos = 0
|
||||||
self.simDrivingEncoderPos = 0
|
self.simDrivingEncoderPos = 0
|
||||||
|
|
||||||
def getState(self) -> wpimath.kinematics.SwerveModuleState:
|
def getVelocity(self) -> wpimath.SwerveModuleVelocity:
|
||||||
"""Returns the current state of the module.
|
"""Returns the current state of the module.
|
||||||
|
|
||||||
:returns: The current state of the module.
|
:returns: The current state of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModuleState(
|
return wpimath.SwerveModuleVelocity(
|
||||||
self.driveEncoder.getRate(),
|
self.driveEncoder.getRate(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
|
def getPosition(self) -> wpimath.SwerveModulePosition:
|
||||||
"""Returns the current position of the module.
|
"""Returns the current position of the module.
|
||||||
|
|
||||||
:returns: The current position of the module.
|
:returns: The current position of the module.
|
||||||
"""
|
"""
|
||||||
return wpimath.kinematics.SwerveModulePosition(
|
return wpimath.SwerveModulePosition(
|
||||||
self.driveEncoder.getDistance(),
|
self.driveEncoder.getDistance(),
|
||||||
wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
|
wpimath.Rotation2d(self.turningEncoder.getDistance()),
|
||||||
)
|
)
|
||||||
|
|
||||||
def setDesiredState(
|
def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
|
||||||
self, desiredState: wpimath.kinematics.SwerveModuleState
|
|
||||||
) -> None:
|
|
||||||
"""Sets the desired state for the module.
|
"""Sets the desired state for the module.
|
||||||
|
|
||||||
:param desiredState: Desired state with speed and angle.
|
:param desiredVelocity: Desired state with speed and angle.
|
||||||
"""
|
"""
|
||||||
self.desiredState = desiredState
|
self.desiredVelocity = desiredVelocity
|
||||||
|
|
||||||
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
# Optimize the reference state to avoid spinning further than 90 degrees
|
# Optimize the reference state to avoid spinning further than 90 degrees
|
||||||
self.desiredState.optimize(encoderRotation)
|
self.desiredVelocity.optimize(encoderRotation)
|
||||||
|
|
||||||
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||||||
# direction of travel that can occur when modules change directions. This results in smoother
|
# direction of travel that can occur when modules change directions. This results in smoother
|
||||||
# driving.
|
# driving.
|
||||||
self.desiredState.speed *= (self.desiredState.angle - encoderRotation).cos()
|
self.desiredVelocity.velocity *= (
|
||||||
|
self.desiredVelocity.angle - encoderRotation
|
||||||
|
).cos()
|
||||||
|
|
||||||
# Calculate the drive output from the drive PID controller.
|
# Calculate the drive output from the drive PID controller.
|
||||||
driveOutput = self.drivePIDController.calculate(
|
driveOutput = self.drivePIDController.calculate(
|
||||||
self.driveEncoder.getRate(), self.desiredState.speed
|
self.driveEncoder.getRate(), self.desiredVelocity.velocity
|
||||||
)
|
)
|
||||||
|
|
||||||
driveFeedforward = self.driveFeedforward.calculate(self.desiredState.speed)
|
driveFeedforward = self.driveFeedforward.calculate(
|
||||||
|
self.desiredVelocity.velocity
|
||||||
|
)
|
||||||
|
|
||||||
# Calculate the turning motor output from the turning PID controller.
|
# Calculate the turning motor output from the turning PID controller.
|
||||||
turnOutput = self.turningPIDController.calculate(
|
turnOutput = self.turningPIDController.calculate(
|
||||||
self.turningEncoder.getDistance(), self.desiredState.angle.radians()
|
self.turningEncoder.getDistance(), self.desiredVelocity.angle.radians()
|
||||||
)
|
)
|
||||||
|
|
||||||
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
self.driveMotor.setVoltage(driveOutput + driveFeedforward)
|
||||||
self.turningMotor.setVoltage(turnOutput)
|
self.turningMotor.setVoltage(turnOutput + turnFeedforward)
|
||||||
|
|
||||||
def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
|
def getAbsoluteHeading(self) -> wpimath.Rotation2d:
|
||||||
return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
|
return wpimath.Rotation2d(self.turningEncoder.getDistance())
|
||||||
|
|
||||||
def log(self) -> None:
|
def log(self) -> None:
|
||||||
state = self.getState()
|
state = self.getVelocity()
|
||||||
|
|
||||||
table = "Module " + str(self.moduleNumber) + "/"
|
table = "Module " + str(self.moduleNumber) + "/"
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
@@ -178,15 +173,17 @@ class SwerveModule:
|
|||||||
table + "Steer Target Degrees",
|
table + "Steer Target Degrees",
|
||||||
math.degrees(self.turningPIDController.getSetpoint()),
|
math.degrees(self.turningPIDController.getSetpoint()),
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
|
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Velocity Target Feet", self.desiredState.speed_fps
|
table + "Drive Velocity Feet", state.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Drive Voltage", self.driveMotor.get() * 12.0
|
table + "Drive Velocity Target Feet", self.desiredVelocity.velocity_fps
|
||||||
)
|
)
|
||||||
wpilib.SmartDashboard.putNumber(
|
wpilib.SmartDashboard.putNumber(
|
||||||
table + "Steer Voltage", self.turningMotor.get() * 12.0
|
table + "Drive Throttle", self.driveMotor.getThrottle() * 12.0
|
||||||
|
)
|
||||||
|
wpilib.SmartDashboard.putNumber(
|
||||||
|
table + "Steer Throttle", self.turningMotor.getThrottle() * 12.0
|
||||||
)
|
)
|
||||||
|
|
||||||
def simulationPeriodic(self) -> None:
|
def simulationPeriodic(self) -> None:
|
||||||
@@ -199,7 +196,9 @@ class SwerveModule:
|
|||||||
driveSpdRaw = (
|
driveSpdRaw = (
|
||||||
driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
|
driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
|
||||||
)
|
)
|
||||||
turnSpdRaw = turnVoltage / 0.7
|
turnSpdRaw = (
|
||||||
|
turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0)
|
||||||
|
)
|
||||||
driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
|
driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
|
||||||
turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
|
turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
|
||||||
self.simDrivingEncoderPos += 0.02 * driveSpd
|
self.simDrivingEncoderPos += 0.02 * driveSpd
|
||||||
|
|||||||
@@ -22,4 +22,5 @@ set PYTHONPATH=%PHOTONLIBPY_ROOT%
|
|||||||
cd %~1
|
cd %~1
|
||||||
|
|
||||||
:: Run the example
|
:: Run the example
|
||||||
|
mypy --show-column-numbers --config-file ../../photon-lib/py/pyproject.toml .
|
||||||
robotpy sim
|
robotpy sim
|
||||||
|
|||||||
@@ -1,3 +1,4 @@
|
|||||||
|
set -e
|
||||||
# Check if the first argument is provided
|
# Check if the first argument is provided
|
||||||
if [ $# -eq 0 ]
|
if [ $# -eq 0 ]
|
||||||
then
|
then
|
||||||
@@ -9,4 +10,5 @@ fi
|
|||||||
cd $1
|
cd $1
|
||||||
|
|
||||||
# Run the example
|
# Run the example
|
||||||
|
mypy --show-column-numbers --config-file ../../photon-lib/py/pyproject.toml .
|
||||||
python3 robot.py sim
|
python3 robot.py sim
|
||||||
|
|||||||
@@ -32,6 +32,5 @@ namespace photon {
|
|||||||
|
|
||||||
// Versions of dependant libraries
|
// Versions of dependant libraries
|
||||||
const char* wpilibTargetVersion = "${wpilibVersion}";
|
const char* wpilibTargetVersion = "${wpilibVersion}";
|
||||||
const char* opencvTargetVersion = "${opencvVersion}";
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -32,10 +32,8 @@ dependencies {
|
|||||||
implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion"
|
implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion"
|
||||||
implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion"
|
implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion"
|
||||||
implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion"
|
implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion"
|
||||||
implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, openCVversion)
|
implementation wpilibTools.deps.wpilibOpenCvJava(openCVversion)
|
||||||
|
|
||||||
implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion
|
|
||||||
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion
|
|
||||||
implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion
|
implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion
|
||||||
annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion
|
annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion
|
||||||
implementation group: "io.avaje", name: "avaje-jsonb-jackson", version: avajeJsonbVersion
|
implementation group: "io.avaje", name: "avaje-jsonb-jackson", version: avajeJsonbVersion
|
||||||
|
|||||||
@@ -6,10 +6,7 @@ nativeUtils.withCrossSystemCore()
|
|||||||
// Configure WPI dependencies.
|
// Configure WPI dependencies.
|
||||||
nativeUtils.wpi.configureDependencies {
|
nativeUtils.wpi.configureDependencies {
|
||||||
wpiVersion = wpilibVersion
|
wpiVersion = wpilibVersion
|
||||||
wpimathVersion = wpimathVersion
|
|
||||||
opencvYear = 'frc'+openCVYear
|
|
||||||
opencvVersion = openCVversion
|
opencvVersion = openCVversion
|
||||||
niLibVersion = "2026.1.0"
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Configure warnings and errors
|
// Configure warnings and errors
|
||||||
|
|||||||
@@ -148,10 +148,8 @@ dependencies {
|
|||||||
implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion"
|
implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion"
|
||||||
implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion"
|
implementation "org.wpilib.apriltag:apriltag-java:$wpilibVersion"
|
||||||
implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion"
|
implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion"
|
||||||
implementation wpilibTools.deps.wpilibOpenCvJava("frc" + openCVYear, openCVversion)
|
implementation wpilibTools.deps.wpilibOpenCvJava(openCVversion)
|
||||||
|
|
||||||
implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: jacksonVersion
|
|
||||||
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: jacksonVersion
|
|
||||||
implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion
|
implementation group: "io.avaje", name: "avaje-jsonb", version: avajeJsonbVersion
|
||||||
annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion
|
annotationProcessor group: "io.avaje", name: "avaje-jsonb-generator", version: avajeJsonbVersion
|
||||||
|
|
||||||
|
|||||||
@@ -1,42 +0,0 @@
|
|||||||
import java.nio.file.Path
|
|
||||||
import java.time.LocalDateTime
|
|
||||||
import java.time.format.DateTimeFormatter
|
|
||||||
|
|
||||||
ext.getCurrentVersion = {
|
|
||||||
String tagIsh = "dev-unknown"
|
|
||||||
try {
|
|
||||||
tagIsh = providers.exec {
|
|
||||||
commandLine 'git', 'describe', '--tags', '--match=v*'
|
|
||||||
}.standardOutput.asText.get().trim().toLowerCase()
|
|
||||||
} catch (Exception ignored) {
|
|
||||||
tagIsh = "dev-unknown"
|
|
||||||
}
|
|
||||||
|
|
||||||
// Dev tags: v2021.1.6-3-gf922466d
|
|
||||||
// We're specifically looking to capture the middle -3-
|
|
||||||
boolean isDev = tagIsh.matches(".*-[0-9]*-g[0-9a-f]*")
|
|
||||||
if (isDev && !tagIsh.startsWith("dev-")) tagIsh = "dev-" + tagIsh
|
|
||||||
println("Picked up version: " + tagIsh)
|
|
||||||
return tagIsh
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!ext.has("versionString")) {
|
|
||||||
ext.versionString = getCurrentVersion()
|
|
||||||
}
|
|
||||||
|
|
||||||
ext.writePhotonVersionFile = {File versionFileIn, Path path, String version ->
|
|
||||||
println("Writing " + version + " to " + path.toAbsolutePath().toString())
|
|
||||||
String date = DateTimeFormatter.ofPattern("yyyy-M-d hh:mm:ss").format(LocalDateTime.now())
|
|
||||||
File versionFileOut = new File(path.toAbsolutePath().toString())
|
|
||||||
versionFileOut.delete()
|
|
||||||
|
|
||||||
def read = versionFileIn.text
|
|
||||||
.replace('${version}', version)
|
|
||||||
.replace('${date}', date)
|
|
||||||
.replace('${wpilibVersion}', wpilibVersion)
|
|
||||||
// Note that OpenCV is usually {VERSION}-{some suffix}, we just want the first bit
|
|
||||||
.replace('${opencvVersion}', openCVversion.split("-").first())
|
|
||||||
if (!versionFileOut.parentFile.exists()) versionFileOut.parentFile.mkdirs()
|
|
||||||
if (!versionFileOut.exists()) versionFileOut.createNewFile()
|
|
||||||
versionFileOut.write(read)
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user