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:
Sam Freund
2026-05-26 21:47:48 -04:00
committed by GitHub
parent d3de87f72b
commit e9006a2803
97 changed files with 732 additions and 1139 deletions

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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")

View File

@@ -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}

View File

@@ -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

View File

@@ -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++) {

View File

@@ -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
} }

View File

@@ -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");

View File

@@ -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() {

View File

@@ -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());

View File

@@ -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);
} }

View File

@@ -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);

View File

@@ -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());
} }
} }

View File

@@ -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);
}
}

View File

@@ -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();
}
}

View File

@@ -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);
}
}

View File

@@ -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);
}
}

View File

@@ -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);
}
}

View File

@@ -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);

View File

@@ -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;
} }

View File

@@ -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

View File

@@ -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 {

View File

@@ -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);

View File

@@ -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

View File

@@ -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();

View File

@@ -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;

View File

@@ -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);
} }

View File

@@ -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();

View File

@@ -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)

View File

@@ -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

View File

@@ -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

View File

@@ -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:

View File

@@ -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())
),
) )

View File

@@ -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

View File

@@ -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

View File

@@ -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)

View File

@@ -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

View File

@@ -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:
""" """

View File

@@ -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.

View File

@@ -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

View File

@@ -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)

View File

@@ -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

View File

@@ -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())

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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")

View File

@@ -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,

View File

@@ -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)

View File

@@ -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):

View File

@@ -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")

View File

@@ -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)

View File

@@ -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,

View File

@@ -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"

View File

@@ -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);
} }
} }

View File

@@ -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);
} }

View File

@@ -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,

View File

@@ -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);

View File

@@ -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());

View File

@@ -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()) {

View File

@@ -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());

View File

@@ -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

View File

@@ -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());

View File

@@ -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);

View File

@@ -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")

View File

@@ -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:

View File

@@ -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,

View File

@@ -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(

View File

@@ -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.

View File

@@ -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};
}; };

View File

@@ -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.

View File

@@ -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};
}; };

View File

@@ -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()
} }
} }

View File

@@ -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.

View File

@@ -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};
}; };

View File

@@ -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
} }

View File

@@ -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
} }

View File

@@ -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()
} }
} }

View File

@@ -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
} }

View File

@@ -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());
} }
/** /**

View File

@@ -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)

View File

@@ -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")

View File

@@ -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:

View File

@@ -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)

View File

@@ -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")

View File

@@ -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:

View File

@@ -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)

View File

@@ -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(

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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}";
} }
} }

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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)
}