From ffd6bcfeffa2b07f15c41cd1986955336c4993e1 Mon Sep 17 00:00:00 2001 From: Lavi Date: Wed, 7 Jan 2026 09:01:02 +0200 Subject: [PATCH] Full codebase for review --- .gitattributes | 2 + .github/workflows/gradle.yml | 35 ++ .github/workflows/linter.yml | 59 +++ .gitignore | 186 +++++++ .gitmodules | 3 + .idea/saveactions_settings.xml | 13 + .pathplanner/settings.json | 7 + ...Build & Deploy Robot for Debugging.run.xml | 24 + .run/Build & Deploy Robot.run.xml | 24 + .run/Build & Run Simulate Java.run.xml | 23 + .run/Build Robot.run.xml | 24 + ...Build & Deploy Robot for Debugging.run.xml | 25 + .run/Clean Build & Deploy Robot.run.xml | 25 + .run/Clean Build & Run Simulate Java.run.xml | 24 + .run/Clean Build Robot.run.xml | 25 + .run/Clean.run.xml | 24 + .run/Debug Robot via IP.run.xml | 16 + .run/Debug Robot via USB.run.xml | 12 + .vscode/launch.json | 21 + .vscode/settings.json | 60 +++ .wpilib/wpilib_preferences.json | 6 + README.md | 34 ++ WPILib-License.md | 24 + build.gradle | 131 +++++ files/images/githublogo.png | Bin 0 -> 13765 bytes gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43583 bytes gradle/wrapper/gradle-wrapper.properties | 7 + gradlew | 252 +++++++++ gradlew.bat | 94 ++++ settings.gradle | 30 ++ src/main/deploy/logs/.gitkeep | 0 src/main/deploy/pathplanner/navgrid.json | 1 + src/main/deploy/pathplanner/settings.json | 36 ++ src/main/java/frc/trigon/lib | 1 + src/main/java/frc/trigon/robot/Main.java | 17 + src/main/java/frc/trigon/robot/Robot.java | 95 ++++ .../java/frc/trigon/robot/RobotContainer.java | 87 ++++ .../robot/commands/CommandConstants.java | 122 +++++ .../GamePieceAutoDriveCommand.java | 80 +++ .../commandclasses/IntakeAssistCommand.java | 190 +++++++ .../commandclasses/LEDAutoSetupCommand.java | 72 +++ .../commandfactories/AutonomousCommands.java | 149 ++++++ .../commandfactories/GeneralCommands.java | 65 +++ .../robot/constants/AutonomousConstants.java | 87 ++++ .../robot/constants/CameraConstants.java | 20 + .../robot/constants/FieldConstants.java | 47 ++ .../trigon/robot/constants/LEDConstants.java | 11 + .../robot/constants/OperatorConstants.java | 37 ++ .../robot/constants/RobotConstants.java | 17 + .../ObjectDetectionCamera.java | 108 ++++ .../ObjectDetectionCameraConstants.java | 10 + .../ObjectDetectionCameraIO.java | 27 + .../ObjectPoseEstimator.java | 200 ++++++++ .../io/PhotonObjectDetectionCameraIO.java | 85 ++++ .../io/SimulationObjectDetectionCameraIO.java | 159 ++++++ .../simulatedfield/SimulatedGamePiece.java | 102 ++++ .../SimulatedGamePieceConstants.java | 47 ++ .../SimulationFieldHandler.java | 159 ++++++ .../SimulationScoringHandler.java | 27 + .../apriltagcamera/AprilTagCamera.java | 184 +++++++ .../AprilTagCameraConstants.java | 66 +++ .../apriltagcamera/AprilTagCameraIO.java | 33 ++ .../io/AprilTagLimelightIO.java | 63 +++ .../io/AprilTagPhotonCameraIO.java | 188 +++++++ .../io/AprilTagSimulationCameraIO.java | 16 + .../poseestimator/PoseEstimator.java | 287 +++++++++++ .../poseestimator/PoseEstimatorConstants.java | 11 + .../poseestimator/StandardDeviations.java | 31 ++ .../RelativeRobotPoseSource.java | 72 +++ .../RelativeRobotPoseSourceConstants.java | 7 + .../RelativeRobotPoseSourceIO.java | 29 ++ .../RelativeRobotPoseSourceSimulationIO.java | 17 + .../io/RelativeRobotPoseSourceT265IO.java | 67 +++ .../robot/subsystems/MotorSubsystem.java | 171 +++++++ .../robot/subsystems/swerve/Swerve.java | 396 +++++++++++++++ .../subsystems/swerve/SwerveCommands.java | 163 ++++++ .../subsystems/swerve/SwerveConstants.java | 122 +++++ .../swerve/swervemodule/SwerveModule.java | 224 ++++++++ .../swervemodule/SwerveModuleConstants.java | 126 +++++ src/main/python/keyboard_to_nt.py | 33 ++ src/main/python/t265_manager.py | 58 +++ vendordeps/AdvantageKit.json | 35 ++ vendordeps/PathplannerLib-2025.2.7.json | 38 ++ vendordeps/Phoenix5-frc2025-latest.json | 171 +++++++ vendordeps/Phoenix6-frc2025-latest.json | 479 ++++++++++++++++++ vendordeps/REVLib-2025.0.1.json | 71 +++ vendordeps/WPILibNewCommands.json | 38 ++ vendordeps/libgrapplefrc2025.json | 74 +++ vendordeps/photonlib.json | 71 +++ 89 files changed, 6609 insertions(+) create mode 100644 .gitattributes create mode 100644 .github/workflows/gradle.yml create mode 100644 .github/workflows/linter.yml create mode 100644 .gitignore create mode 100644 .gitmodules create mode 100644 .idea/saveactions_settings.xml create mode 100644 .pathplanner/settings.json create mode 100644 .run/Build & Deploy Robot for Debugging.run.xml create mode 100644 .run/Build & Deploy Robot.run.xml create mode 100644 .run/Build & Run Simulate Java.run.xml create mode 100644 .run/Build Robot.run.xml create mode 100644 .run/Clean Build & Deploy Robot for Debugging.run.xml create mode 100644 .run/Clean Build & Deploy Robot.run.xml create mode 100644 .run/Clean Build & Run Simulate Java.run.xml create mode 100644 .run/Clean Build Robot.run.xml create mode 100644 .run/Clean.run.xml create mode 100644 .run/Debug Robot via IP.run.xml create mode 100644 .run/Debug Robot via USB.run.xml create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 README.md create mode 100644 WPILib-License.md create mode 100644 build.gradle create mode 100644 files/images/githublogo.png create mode 100644 gradle/wrapper/gradle-wrapper.jar create mode 100644 gradle/wrapper/gradle-wrapper.properties create mode 100644 gradlew create mode 100644 gradlew.bat create mode 100644 settings.gradle create mode 100644 src/main/deploy/logs/.gitkeep create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/settings.json create mode 160000 src/main/java/frc/trigon/lib create mode 100644 src/main/java/frc/trigon/robot/Main.java create mode 100644 src/main/java/frc/trigon/robot/Robot.java create mode 100644 src/main/java/frc/trigon/robot/RobotContainer.java create mode 100644 src/main/java/frc/trigon/robot/commands/CommandConstants.java create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java create mode 100644 src/main/java/frc/trigon/robot/constants/AutonomousConstants.java create mode 100644 src/main/java/frc/trigon/robot/constants/CameraConstants.java create mode 100644 src/main/java/frc/trigon/robot/constants/FieldConstants.java create mode 100644 src/main/java/frc/trigon/robot/constants/LEDConstants.java create mode 100644 src/main/java/frc/trigon/robot/constants/OperatorConstants.java create mode 100644 src/main/java/frc/trigon/robot/constants/RobotConstants.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java create mode 100644 src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java create mode 100644 src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java create mode 100644 src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java create mode 100644 src/main/python/keyboard_to_nt.py create mode 100644 src/main/python/t265_manager.py create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/PathplannerLib-2025.2.7.json create mode 100644 vendordeps/Phoenix5-frc2025-latest.json create mode 100644 vendordeps/Phoenix6-frc2025-latest.json create mode 100644 vendordeps/REVLib-2025.0.1.json create mode 100644 vendordeps/WPILibNewCommands.json create mode 100644 vendordeps/libgrapplefrc2025.json create mode 100644 vendordeps/photonlib.json diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000..dfe07704 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml new file mode 100644 index 00000000..796c3a29 --- /dev/null +++ b/.github/workflows/gradle.yml @@ -0,0 +1,35 @@ +name: Java CI with Gradle + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +permissions: + contents: read + +jobs: + build: + runs-on: ubuntu-latest + + steps: + # FIX 1: Move submodules here and update to v4 + - uses: actions/checkout@v4 + with: + submodules: recursive + + - name: Set up JDK 17 + uses: actions/setup-java@v3 + with: + java-version: '17' + distribution: 'temurin' + + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # FIX 2: Clean up the Gradle action (removed submodules from here) + - name: Build with Gradle + uses: gradle/gradle-build-action@v2 + with: + arguments: build diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml new file mode 100644 index 00000000..61e0e47b --- /dev/null +++ b/.github/workflows/linter.yml @@ -0,0 +1,59 @@ +--- +################################# +################################# +## Super Linter GitHub Actions ## +################################# +################################# +name: Lint Code Base + +# +# Documentation: +# https://docs.github.com/en/actions/learn-github-actions/workflow-syntax-for-github-actions +# + +############################# +# Start the job on all push # +############################# +on: + push: + branches-ignore: [ master, main ] + # Remove the line above to run when pushing to master + pull_request: + branches: [ master, main ] + +############### +# Set the Job # +############### +jobs: + build: + # Name the Job + name: Lint Code Base + # Set the agent to run on + runs-on: ubuntu-latest + + ################## + # Load all steps # + ################## + steps: + ########################## + # Checkout the code base # + ########################## + - name: Checkout Code + uses: actions/checkout@v3 + with: + submodules: recursive + # Full git history is needed to get a proper + # list of changed files within `super-linter` + fetch-depth: 0 + ################################ + # Run Linter against code base # + ################################ + - name: Lint Code Base + uses: github/super-linter/slim@v4 + env: + VALIDATE_ALL_CODEBASE: false + VALIDATE_EDITORCONFIG: true + # Change to 'master' if your main branch differs + DEFAULT_BRANCH: main + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..c080ae2d --- /dev/null +++ b/.gitignore @@ -0,0 +1,186 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +simgui-ds.json +*-window.json +*.wpilog + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..93a08b0c --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/main/java/frc/trigon/lib"] + path = src/main/java/frc/trigon/lib + url = https://github.com/Programming-TRIGON/TRIGONLib diff --git a/.idea/saveactions_settings.xml b/.idea/saveactions_settings.xml new file mode 100644 index 00000000..6025467f --- /dev/null +++ b/.idea/saveactions_settings.xml @@ -0,0 +1,13 @@ + + + + + + \ No newline at end of file diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 00000000..79abe24f --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,7 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [] +} \ No newline at end of file diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml new file mode 100644 index 00000000..a5a4ac4b --- /dev/null +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml new file mode 100644 index 00000000..e5011315 --- /dev/null +++ b/.run/Build & Deploy Robot.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Build & Run Simulate Java.run.xml b/.run/Build & Run Simulate Java.run.xml new file mode 100644 index 00000000..1009ba76 --- /dev/null +++ b/.run/Build & Run Simulate Java.run.xml @@ -0,0 +1,23 @@ + + + + + + + true + true + false + + + \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml new file mode 100644 index 00000000..6d077974 --- /dev/null +++ b/.run/Build Robot.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml new file mode 100644 index 00000000..2425e0cb --- /dev/null +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -0,0 +1,25 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml new file mode 100644 index 00000000..77bdc975 --- /dev/null +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -0,0 +1,25 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Clean Build & Run Simulate Java.run.xml b/.run/Clean Build & Run Simulate Java.run.xml new file mode 100644 index 00000000..0c27fea9 --- /dev/null +++ b/.run/Clean Build & Run Simulate Java.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + + + \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml new file mode 100644 index 00000000..8d57834e --- /dev/null +++ b/.run/Clean Build Robot.run.xml @@ -0,0 +1,25 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml new file mode 100644 index 00000000..1ebb356a --- /dev/null +++ b/.run/Clean.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Debug Robot via IP.run.xml b/.run/Debug Robot via IP.run.xml new file mode 100644 index 00000000..3bdaf262 --- /dev/null +++ b/.run/Debug Robot via IP.run.xml @@ -0,0 +1,16 @@ + + + + + \ No newline at end of file diff --git a/.run/Debug Robot via USB.run.xml b/.run/Debug Robot via USB.run.xml new file mode 100644 index 00000000..fff0c0e4 --- /dev/null +++ b/.run/Debug Robot via USB.run.xml @@ -0,0 +1,12 @@ + + + + + \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..c9c9713d --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..612cdd0d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..1e540470 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 5990 +} \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 00000000..dde220be --- /dev/null +++ b/README.md @@ -0,0 +1,34 @@ +
+ Logo +
+ +# RobotCodeXXXX + +TRIGON 5990's robot code for the XXXX season robot, "PLACEHOLDER". + +# External Programs + +All the "Advantage Scope" related files (CAD glTF files, layouts, etc.) can be +found [here](https://www.youtube.com/watch?v=dQw4w9WgXcQ). +Credit to team 6328 Mechanical Advantage, for "Advantage Kit" and "Advantage Scope". +We use these a lot and highly recommend them! + +# TRIGONLib + +[TRIGON 5990's library](https://github.com/Programming-TRIGON/TRIGONLib) for +automatic Advantage Kit and Simulation +hardware wrappers, utilities, commands, and more. + +## 📞 Contact Us + +Have questions? Want to learn more? Reach out to us! + +| Platform | Link | +|------------------|--------------------------------------------------------------| +| 🌐 **Website** | [trigon5990.org](https://trigon5990.com) | +| 💻 **GitHub** | [@Programming-TRIGON](https://github.com/Programming-TRIGON) | +| 📸 **Instagram** | [@trigon5990](https://www.instagram.com/trigon5990/) | +| 🎥 **YouTube** | [Trigon 5990](https://www.youtube.com/@Trigon5990) | +| 💼 **LinkedIn** | [Trigon 5990](https://www.linkedin.com/company/trigon5990/) | +| 📘 **Facebook** | [Trigon 5990](https://www.facebook.com/trigon5990/) | +| 📧 **Email** | [trigon5990@gmail.com](mailto:trigon5990@gmail.com) | diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 00000000..645e5425 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle new file mode 100644 index 00000000..8733b184 --- /dev/null +++ b/build.gradle @@ -0,0 +1,131 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.3.2" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.trigon.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141 + final MAX_JAVA_HEAP_SIZE_MB = 100; + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + + // Set the minimum heap size to the maximum heap size to avoid resizing + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Configuration for AdvantageKit +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() + maven { url "https://jitpack.io" } +} + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + implementation 'com.google.code.gson:gson:2.10.1' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} \ No newline at end of file diff --git a/files/images/githublogo.png b/files/images/githublogo.png new file mode 100644 index 0000000000000000000000000000000000000000..9e54508d4636940d6c71e48c5d4b5f0dee9e2457 GIT binary patch literal 13765 zcmdUWWmr^g)GmmGfP{1l(hS`#4N`)nl(gi~ISeXDBi$vRIbv_YQrprtlJz91{r%>7|mQtR@l?3J7?ggMkM8%$-*%0)8R8 zYAQ%0m5)&F03V)POQ}jBAyq|T-J7BUpIboK#;lL0tJsKXTXzVu)F-CqI zH>Ss28G<+zbMt@w?|&vf3;*{gzK@uo|M}8qmj4&Nq=+>stkg_csoZQV~V( zbiuRra{a@l89qM#e@v*v_%S}LXE)Pgm+qQ<9iw6S$_Ep`8Z>bcOL50|{_Ha6qu=qY zUG2jI?hlr;8jo8y5WHFtI60;*4^V(+FhzNLs)_&Oj4`F@++c{$U3tiq5ZZq zz`@!asuCpUis6Pf#Y4T-7x=E;lxI=i>bH zt%)De_xe9^PC`RRagrP7PB(w61T3M@+sC^Q0ff^3nTTH^E2>IeT(09HjSC6u^NQ2y zMxN4TL3ekz(K2YL3RtX0%$wy5(ngY^^W+N(U<^V25gfIbX zt=(MWlM3nR1~_Uh@$N9!e$FE{BCJ7aoW3OS`xTON=W8t$ni6Fpyf3X{NR&7MfoZ{} zt(U9;nf{6M^M+wzVf#jaIc2}f$bOxI{1}wk^d?u~$qm}D z(`ZwDMsK1w^d9B!Z!oZkvhxLa{ja~~h0V5SG83~KhG=syr~tR$E%@iq-@}Qye}91D zV12RqSeL0{qN5{UNmr@&w&;gOGQZuVR{Ki5bMrBH^uMZ$f6<7D9W(P(MuW++!Wx*i zexha-^HO-YJ_20RM z93<`6Ue6{bllk~52}hHq81i4na4eYYCsLOI?WO2OuSquW48zFdSMf^bej zzmH)d(?XOu&6m49wVx)M*r;L4@dw@zF{rfc+`(M^pCMg|m!%~zee(=u!2cy%uU{EG z%~LmZZ3aG%jf;CT2xu#h5s`5|ru&WvLtfFy7}v?2Gbwnd+}-XCnN!>CO+UL~rWY1?es)*S>&N=pBM5X+fGap`D;4O62QwiB=0C6Yn-tTGPFg5*l|(4=PdD-O3C`H zE&hxe)jgbCJ@T|4O=L7~f1bS;3&(2XZe-h)PVCwK5@l`zjltq*{8=4^NWQ9oms&6- zR_JvW)r^>Gy;gCnvoVqafl45M^F^?jYe3kq2f0|UP;0(;!r#q6{-BDT{QV#+r~RE0 zk*3oM0z*UgWs^oSgde`%-E?8g#kX+V1`@HVHAe|xw@!3n{?g$c3WMQptSC@x=mDq9 zpdj*hi_=attpzsO(3x6453DY;PBTdpZf3>x>o`EO=_aL6NkyghUnjRIh?@nJEJ@#z*`jd?&5{}))hBuvcqARH@*Ze!j=sl2IW>6Ry7$Ds4 ztl_i}LRVFAHo=Cu+erw_B*^htpx%GC_FQ zXka>?-4TBuiA zUEk@3^KU71WG>5jp=KiDs%~t2bD?As%xcpcm#&q{2OVH()k%oJvaRws_!zYK$hAaR z51ZIzrX*ro2oCqaqoR8GR>S#)-7{#lv{e0fm_?9uX{KP*d*;(2VuZ1L5ylem2k8$w zjJBz3^tm203I^Mv%cd6S-yGGTnek;n7O#+9bx)W!g7{U3Qdt3inzCIgYV zeFWQHXqk-`z64@bp@D4j?^7FWzxO#gxvNk{bu;`K+o59@rwWz&Qs@%FTQfUM+z$#*$?=fnGNVi zG=Wr2ARaXldp_kckng5mcBH)=M#sw->oR_+_gp!gdAgPo&J+61Hi(IX3=A0#pk;@5 zV+M1{NIhdf-Pe2MZ3-_~!A$&pvJU6M*$r#3vFkD=fLB+7ut0Iq6?p!aQSH3-)m}0o0`&YstNK{Iw^SDOqQm!Jsam67#{91(Lt+& z?CXCSKVTFVCIf55x23*k?*2g~I$h;J!I2c+&8Yf0Huf7_yPZr%N($N1(lX(|$Wf4G zV1TscriBPUrWwd}{m9_c!Ym$_Rk5)j9rR{jAF?n7IPyQ8X==8dGce}Ml{t7Yu5&W@ zu6T9}sJ?0P!~J>q*i^FRYPp3cxaic@R@}LkIyFYoqtQ&@v!evK{Fb7gw zi`&X(E1ho*266P7Gj~VdiWsj4h~1#RwDjx-g57*=Hj>3KqM)(mb1os6Xtwvb&%Z{#IN2Af z$BmMB6k`9Vg{lj$CIxPhX6&7SBYKKKj@H4*^xJ*={bA!8j-eWp=Sv#3aT=@TGQAGI zGHqpv`(v3`_Oz<9vX|)ykEnd7tbF6Lt996TwdD4=B{a|d0{7{--Kvra6Ax&9HLXt6j$pD@{Kaa%GC5<>%+mc`~8pl+5zQ z+38=a3Uv?qvdb|%H!?2yq(!Fp<7g~l)KAK{h8*;%d8u#Hf1!T?SM0t1qxC0m*FQXB zM6^zCZD`Nx9m?$ElCnnKNsNXfm34o^4rX@F=1Fl*_VE|UBrV^`-qh}llL=nCWJZKC zEf(G3n0|7pjBww|>MPpHg6+6HtF#&>e;nV^lkcO)(>@Uk&=Xq?G&NDpcP&(#_kg`L z6nzj)HQ2bYrjG9dt^_3S2rq0Pd$_uHxQ2)9_vus^4==NoFCT98uGzY3MOHqmbeI_> zwErj{j6C(1trUqgD4-zYM~=}G+)1HNqtb@Oz)Ib8!{5YWFXssvkD2ACQCM1D9=-3C zUn=sslds+>4EM6w=+q0IwdXLps2ZS9~i%T()p1cv#AKxmXmZlo})+V-M& z@bswt?uXQ2(O~7X<=#(UIDhTtE5rRgT3)0v_Nw)b%F0@R_wN@BWGy5==xKHHj&}SC z-NI!kIhd)HxFDCdlv^*oP8g#iyT)>59{dQqrCBVbx1%~xBO~X=)HG33=L>bf$(O4@ zu00S;mU%qk6jK@=xOE_xBV)74Ab)ylJ~VY;?$7%Y6^cxg^&GOUr@b{~F93sRY6x;K zziF8z9{1uifpsE(cU^u8(u0Fb>=lN^f07GS0XE zb6fbZW@ct4bYBT$oy+u?wXx8qm^l_2oGSYp%*&t9-bY1pl3r|r z-T($4y~_U0V>4UNu0ZW=VVO0u(5#_ZWt^`wA@AUk%y>n#S;a!x6)t7VJk)w~=B%?% z9E9jM1As0n<{{NmU*++fccl6J-8vRbLcI_cw9pe`?>gJ#5-G<0z_sF0RF0JpP-y@d-*z?!k+T z+maF-v@kRWZkwpg?y2Ws-1n$qFBbcmj<;cXaMWRbXyFG`tbe0`h3_Y+jIS@85RhIR zCe=#ZIhv)`RSC|RTM2dV{>MpxqeX?+SlES3BV}^JakLWpV=cPI(Xtm!a>~#jrKzb2 z5Gf2RdH>i69#zwLkR&{R38y?Xss1MA$f*_a?s>6lnysR80c3>8M0KsVqQml7cU%wX zluU?U8+Z4G!zY%RMU+D03- zgWwkMimWv!ZzrF@jzAPb>x%Ht4Qwl4vZ6cU$I^%G4S3>yb{z6k1;Lbpzdl(wCJr2U zE|NLN`j`_skm4szaj{BT2n531MCzl;6ktwB+(8=N6JTtIt#mXYDjX;e>^Up(Ov5PK zr;}KQ%K|#OvA006XVJmyaO)=DcJrcal=25#Gt`vfeXM$KYen+YpyW*Zxx#Fw7+;PA zCG(r@xf`3K3Gda|5H(wQ2pos+z~kBXG~G(`i)ZiZ9U}COPMCs4!$!822F`oP<|WN! zVQTKSpWxC`#bAO;5aIeu6YkMQs#ZS>b$PKfmAYBKxN%dwjf=5X2LocAQU~F`2Ud%v z`uAkwGIjToVIlpC|J-%3KsAjNx&-4B-SH7SVY%ED*NksUU0P~+Enf;6KTe*}aOn z5{gLpX?+bJY0Z>5xLqWT`k;LoZS1TgNUyvDO8}P>ZTgF zOoWYw4)@E0ij)XOI+tZlL_h`Eq(=Dd8Kq`=lT@jWA563iW8cmk!=4j&JaR5VH%*Ow z9)(+t)yu4_fh4iUIZ*g%ZBhM&i_ijno_m2@#rRB>3rD_i+JWjZBY*DN_p8(OWY25M zNszC5?k7HnLH``Xb!~_<_|VD4;P?8KiT^^O=O4@<=|(mIJ13p=ahFpirmCR>Mr_J; zr{90p3Tn;5k^v_JA)E|sVPSFH*>DzOH#uosHwWVFPUSSoX%S4*s|c=ln2k`PaD4C+aqz%iPq7%3vvY@t-{EU>)x_&Xm7D);uq+4X0?lDx-&`rnvbJ*hcH ziLP$d>XbDe^pJS>7kQZG|(Zthy~V!`i3EBimDWpO;@9wmVK+!R15 z`=M*Op>c}azbX2Rq0W2(&`Ic-h~M}^>uJ~7x%iY&M_GbWiDcB{(eKHRFeyGdCT8Ggr;PV`cIliG08@3jR*c>UchwoK8f#zv9o+nw8u~rqBt6uyuf$INecd9y)YQaVp8LCu^ZCd zyP9JFUcnDmDl$vY4~EOsknyHmoQ+-!=p;1@ZoWMc(NjhS{4 z^*W>eW=cQ4?{(>W1i!k9^m6Ix=q?HYaOm!P1iQhpw`RyY#X8-IK_1w8*#H#7K>;Rmqlezyb#+QB4Sn zE&tpva;0;8q;?j$*?amaB+66kWk#U1?}y*8;EwzC?Zru8Z?^B%A*KHw`#mRNcd_1? za|&!&`D$L-%BrC4M$KJv6{CIo@7M1W#Aj0sC0eRMjEq52u%HvKA`o5e zUq);d_A`9X-nY#j2euVKVf-L}+l0j4h{o-Zj6X&b!2l>)KVndo(x zqEs2(&V=t-kv8TKggA_a;aD$79~mqzCyO#h`z-nkeO{)j6?@Q`J@5Fo?y=z9Xq!bX z+k<1Q+Cm-7#`v`{U{*7|sdv$$LRj4RBc(T5lN~W0^GJA+-RyxE)F2CXM*}6*Fy=%n z#RyIz!Q|m2v?wE6+~GO>V#&?4T+#Yyzvm{>w0pakvRL;v!Lyj;v2T z9>26y-lWlR__umzzlP5JlpX)8Wt=V)lho|Fro?ow`KSE$!j8FLG>|MYwcEX-L&sdO z8f!!{>uW{iMsv3wqrU0pq)XJ1G_ri-ORG$eSGW=neC4*|{eTn4e)=MeEa{2@9SS4y zQ4Ge3&+ul95>_EbLjhr--UtzX>gsgxKD^m_b4W`^=Q~i&!$Z5_&nv&r;P8NiVx}xDw71=`< z=q<5G*u$7bb96aZS8XNXRwMIG*TrSZq3Vi16|N42GSq;+_F%ToexRS(L{F2JQ6Mo> z+-KRNgjOgs9t!J;7qsxY_SRfU9#K zZDp~HjBZutMinSTQS;f1(XUbi@X8fp$IYs@gcDPf-Y{dq9BiDscSz-7xRmH&E(3Fo zfuEF+lTFh@hX-l;On}z?It+_T`Rs(`@O8O9#caBhMu2400oRzD2xJ+lNVnq03vH0gZQc%m)S8)YOe8QS`?bQrDmye!JsyedD15E`m-oHgjK7YRH!AIa}od2njZ?VqcS*vJxxr*8!v&rSQsRB!kOT{?CV z=w`i3D_!!kn8EI?$ddWskDoam*a70}&a3TCnI4=M^bGA*ueUT!9U;CXI$R;PlXUM_6x z!r-DnM*2EYO4!OzW>@|htd3lIS0bFGr1b#b=$chs9P$3)M!}^#cOMa?Br0D}l!ZM* zAD{4&+i&k_J6VQDcdRF6*BXq3^|XCQ}Uwf|cU<%$#X+bdA ze^4sAN!xCu68M|q>_OA}e=zVWIxYnSpfA4FvVUbjGMeW$X#nAMX9ZBL{~-5l>Ox=9 z1+0$cUiV*r75sDG&&#^^h-t;F2;Kas`O&i+gq4s`d6T66*?Ik~2jq$T>COpT^@Hb; z7?3ezdjH3D08I33UwEE>mq_SZHq}j}xte4B3ZHq9f$|uim5pwl6Go6Et0m2vdCiz0 zh4y-#&FmopmszhPdQ%)ddVU4NEG;oACw>ELenJ7eUd{Cs-rO#}LopohkhHEa?tA0l z6YFz=L0Mk>Vd|+vuV7#n-1p}(kL@^VeVoINyh7rEbg`(o=LWFm9c!5>evDr#Y-Krc z?Uu`Ch&k0Ed9ar=tI%Q(4a6J4#+sV^SI6sBYIKV!3z=)j^Fj)++4x}PD^@nsxmcQ; zgNv2;4g1Yw+NhYxt$6@Ci0*&_JCc(&fL=t~rOvM&zw*3|^jFqH=Ay(gORMeHO;>jt zld_<{FdN@wzqCS#K##B+XRVt0Rxa-1RwVuue#^Q{BJ>!KVP+42`usIG^cXq7d_6OJ z7&m^&*{}6Go}y4I7E~_3fwv)@3vMm@v|`R1qTjT2Tdf30jJ&?Kk;1E|cSGnLjup*P zYio6%I>H=hgQ{E0kdjZzu6=s#eG#(t9vJhKEtl-!x>zSrNs z2(*s8EXq47!CwWJLzNddn%L)(9%ZLuy<5*N=kP_rWrlTO;o#P|%`J%#*hc8~nWsLF zGvVZ=HYu&74SGJ$ULV=NdFCA@ph@AMAd~iV+J;5-RTkJuH^W?sogU8(%ORB;rQm{> zx$I5l+>urLpUC(CU-|f_44rmQ5>5#(wl<&3JpCFgp$z|{BA|a%lmWo|7?hE2b%~Y5 zKNVw#_x=P4(|?VP26$9AJ8sn(TaRhb(tT_#y>&bU`ZD>G)pR2q`EuEi_6pnkTX2_t zX|(yymF&hRWvOcbb6c;%oZMWS+1kycVLnw%EG!mQ)~?4tt1c}{YVYVoL_~7tIq)#v zzkmN_bgM>p<-$ivO|AD}@n#bZXub&v34sA99UFGQbTTP0EOk?h4C$9Zj16&wfH-;# zra8T~qxo|hF&Fyovl;l=+uyp~_qvW28|gUT^5_RR$V62!vy^wA;!>6GD-@;S0GTaknol|+DM~b4z75-!BKNK z&{1iZuvzO9XF)H?vN|q^?Bq7s%Lj-STRX$WY1<3NCOhaGn**(a2TvE-C8kVBY?N+t?R7YA==_Jhc1Vow zVPkxpy2OmJHrqI#_s&=R^s`5WZnBmJoAufE(N%4}rJaX&_u$@U%0S%COs<0L z!`)F^pZe9}D2gNJqKN!^&?ilOmNT_&cuhqlgMPN0Y~G)_Hg#s8L5wgZP4_%(g4}Mp z8vfz#zx*$EhU*LkQ-J*qctKhRa*KV#+k4vGkG^j-Q|-;0xlVk!>KChVB2{^pr12)Y z^>jThy~T^=LEJc_)V^CrrKTLRM0~}__#}BtphFYOYwvz-u8U!(bVwWxY)QCN$YdrPns(~ zvvMM|3GI#5DoeYKUlP%lO4#wLyq@Eku@AB{OdH~&DB~j2N$wvo<~r&y`Q4{qSDrUr zO?PXln?_A+O`1-IQMKH##0m#SnKlj3H>#}ihM-R^2PbDLHPwEig;-bLPy>rm6P?$3 z|1jZ|QtH1zNtw6>bHn1bRhXTg1(>fU3%lD|`s3r%6l(1L;Fr9U-WSwOgH?UQyx*C+FSR|}Wr_&LpwqMlw+ATNFU5V@ z8EK&LNJVbFzySHrR+%eAvT<^9n#r15cYQ??b#Z08+uYjv;HqBPER4+z0L_K7Ye@-- z%hLffj%tOxtDY-vBHY;MEmxR8&(E!a7|3EZEI}y_GUoIiHrSH0zqI*OB(JG<|1N-Tm1=T+QT`frYBSnYb@)M5q{<| za#@D&D_@|T_D2v#s!24{_*P&iNBAucw91mCN*bHC)8R&|?7S*UBUSg8d=;Az9F1D;(XJ+qeq)~`Nz3B5@RZqPCU z%Djz~i!__7!{>*OTatExiGOw2S&A=bIr$AEhGvc3qs%1BDC@x5*mxu)3i9?7Ut|Bn zVTuwFix`^z=pD6EAZ0HuK!^wI=}>xqFAU_Gxw7R~ z#|hC7cMbO^iw__QPtm@@b)J)iz8j}5uMx>3F*8=Rz^KU3&1GU1y+zrfPYZ8^^XCNP zCaW2oodJMCeD%3uouPA3fO0E$?c*Ujw4@$J@@pW;q*L&*IBw^q!BTVXyQayV9Sir} z{vfA`fpeTW@pDrK03{o<{t}`n-c5=&KL+tkNht_$qge(A93jD@@TY(#lu`iED2qf+(wPq6h|AJf8 zX!^_Gx&VlIOR{m=oy>iHG=-Zr8Z3#21g8V0y#lI0-$&h$w!Qrs=mSHdqWU&wZ<-?j zdQ{w6$LgF4Xa1Z(?AQsjFK`7N=m}kJIaR+LiSg%(vN%h9$o(IMdl3hp4F9&lQTUsO zDdBlas$dj73QX<-PuyN@(H-ia0G-C$b(72P)~((e94TbzqTt98Jd)DK|ECHGLWQ~P z??$g{;%!-{Q|T}#@OU+$3(AnBF;Pqsg0~4ghhs?Uj+U6Af8Iv}ah`qac-s3*?HPPT?B?%>bMF-*#ArB#(rNcLH#H<= zA313}l&$;)1y*)Zo~^iopMIh?bV8^s@0}3gPZ>C4t>eP1ah)>2G6TAHk&16!(xvZ< z%q-s>U}9q0p4XU&8uY!w3HJ1k;>iL>NmK%PNPlJ~;lb6_wG;d3&tyN)!U=s21h#4F zVtGA~8q3hyaX2_zW^|1dV!``EIH#rUh8imvCMMyCY2xT+8owDkwu0eH@Xpg>jNO`? zQ{s+KZEGY)iXNyk4kmO{o(7oQ(J z{JY%D!0ZkKKB!&dbq<$!UwoV^6Eh4mq-1G4Z0~9NXbWSy|cq{QU$jFR+QtFnMR=$f%+Eai#~fAVwPc? zk4)4Go0n0Q&V@3+An)7_?Io4;QoEmBIuY-2Ei(P{U5UT1kO~82e(Voh>f#^<`$DW9 zyE$er;eLWk5px}9zJb^Q`SVYABe)bC*)%AnAP`-xxwWn>eOim6fbD{41$5)IhnL{u7XuwC+Q;Vrw zF@*&~g_cLW$Fn$5wJLGLSy5)a!4=V|spJ|=z+T<)j|L{b5%vrvwI(2talpO?R39X* z7gPlgczqRUlL~p56A|NnA=|?+Q!-$5Mg0D)0RCa zVj6)E+CGz!5Q%_eYA88p+XF2Y^lo_3vI1mM_3hESfeWADC-*oDngN|?%O}i zaqD>iO4Q60f@Qs*4a{1c&IS%nI3J%SjVpb;JIuQf`hNZ>GrhR-ti1UQe{(nud{o~j zk;;=WG!d?3Ptep)_1b^!)z~dgPAEx^7`1O55F=0-JD|?J{AQC1kX@aR;2 zsEgLXeekWIQ2^?#s&NcN+2ABZS!+8Hw(0Cld2sogfB6lg;gE*XcUdMu^TTGU{ez_d zAh9-jUpvL(h(^lJNyV(ZC8KC3gUK`)%dB{EI*(N;u#_UCL~sQ2z5@8`s@WP63fOSi z!P9fqCAMbB~-V)u7?4J2> zx;wi7;+@E zbh>s2qFf)mADVW0DV&AQ>)C-0nb_|Xpi>q^45?64o^fl4bH1)Tx8tzMQ1Gd4sBU*i zo9yoGl|7#!L-fHLNllOScZzp?8T9psXcN!g1HGHKa=>yg(bk9Z1LEwVDg7J>i)c$3pa-R+&S=3u?kW7C$b3Obe^v6XC8 zDhR$B&e^^U^xP}^%E8q<^&|@w(6DXBnDA{sQ6By4Nvd>jy${A-m-+~DqwvpvX##DW z??lwRlS?t;V%1GPGwM}sWcBlB$N~v=l46p&^gm5=)ZlmpDCL>;qT`B?e(3M__vF1SQ5POmlyHZtUwy z34kwwDsepUoM|2Ml-;OJ^0o=m7tJ3TwQ>61%>;p@uUOWNxbCz3sbHSIYBvxmRef6+51 z5Y4^i3K8Y`Jo!L>TkCRmAJD)CL~qFSKM(jyLE{s|VggaEkx_qL2d@H%J2Pprz!MN} zO>rzy=S~j#5!or?ewdJgS!g+IQzkQ& zrU7k;=NkS7{5yYlvW@~4yd>WWQ=03Rb!^GfAN2pd*O0t})R#ckdtBRI+kw)Kr2Lb1T{|G`5+2r>V!4=PPQA)oR{ X@vG-+vuvg^_??!{yS%8zW-#zn-LkA z5&1^$^{lnmUON?}LBF8_K|(?T0Ra(xUH{($5eN!MR#ZihR#HxkUPe+_R8Cn`RRs(P z_^*#_XlXmGv7!4;*Y%p4nw?{bNp@UZHv1?Um8r6)Fei3p@ClJn0ECfg1hkeuUU@Or zDaPa;U3fE=3L}DooL;8f;P0ipPt0Z~9P0)lbStMS)ag54=uL9ia-Lm3nh|@(Y?B`; zx_#arJIpXH!U{fbCbI^17}6Ri*H<>OLR%c|^mh8+)*h~K8Z!9)DPf zR2h?lbDZQ`p9P;&DQ4F0sur@TMa!Y}S8irn(%d-gi0*WxxCSk*A?3lGh=gcYN?FGl z7D=Js!i~0=u3rox^eO3i@$0=n{K1lPNU zwmfjRVmLOCRfe=seV&P*1Iq=^i`502keY8Uy-WNPwVNNtJFx?IwAyRPZo2Wo1+S(xF37LJZ~%i)kpFQ3Fw=mXfd@>%+)RpYQLnr}B~~zoof(JVm^^&f zxKV^+3D3$A1G;qh4gPVjhrC8e(VYUHv#dy^)(RoUFM?o%W-EHxufuWf(l*@-l+7vt z=l`qmR56K~F|v<^Pd*p~1_y^P0P^aPC##d8+HqX4IR1gu+7w#~TBFphJxF)T$2WEa zxa?H&6=Qe7d(#tha?_1uQys2KtHQ{)Qco)qwGjrdNL7thd^G5i8Os)CHqc>iOidS} z%nFEDdm=GXBw=yXe1W-ShHHFb?Cc70+$W~z_+}nAoHFYI1MV1wZegw*0y^tC*s%3h zhD3tN8b=Gv&rj}!SUM6|ajSPp*58KR7MPpI{oAJCtY~JECm)*m_x>AZEu>DFgUcby z1Qaw8lU4jZpQ_$;*7RME+gq1KySGG#Wql>aL~k9tLrSO()LWn*q&YxHEuzmwd1?aAtI zBJ>P=&$=l1efe1CDU;`Fd+_;&wI07?V0aAIgc(!{a z0Jg6Y=inXc3^n!U0Atk`iCFIQooHqcWhO(qrieUOW8X(x?(RD}iYDLMjSwffH2~tB z)oDgNBLB^AJBM1M^c5HdRx6fBfka`(LD-qrlh5jqH~);#nw|iyp)()xVYak3;Ybik z0j`(+69aK*B>)e_p%=wu8XC&9e{AO4c~O1U`5X9}?0mrd*m$_EUek{R?DNSh(=br# z#Q61gBzEpmy`$pA*6!87 zSDD+=@fTY7<4A?GLqpA?Pb2z$pbCc4B4zL{BeZ?F-8`s$?>*lXXtn*NC61>|*w7J* z$?!iB{6R-0=KFmyp1nnEmLsA-H0a6l+1uaH^g%c(p{iT&YFrbQ$&PRb8Up#X3@Zsk zD^^&LK~111%cqlP%!_gFNa^dTYT?rhkGl}5=fL{a`UViaXWI$k-UcHJwmaH1s=S$4 z%4)PdWJX;hh5UoK?6aWoyLxX&NhNRqKam7tcOkLh{%j3K^4Mgx1@i|Pi&}<^5>hs5 zm8?uOS>%)NzT(%PjVPGa?X%`N2TQCKbeH2l;cTnHiHppPSJ<7y-yEIiC!P*ikl&!B z%+?>VttCOQM@ShFguHVjxX^?mHX^hSaO_;pnyh^v9EumqSZTi+#f&_Vaija0Q-e*| z7ulQj6Fs*bbmsWp{`auM04gGwsYYdNNZcg|ph0OgD>7O}Asn7^Z=eI>`$2*v78;sj-}oMoEj&@)9+ycEOo92xSyY344^ z11Hb8^kdOvbf^GNAK++bYioknrpdN>+u8R?JxG=!2Kd9r=YWCOJYXYuM0cOq^FhEd zBg2puKy__7VT3-r*dG4c62Wgxi52EMCQ`bKgf*#*ou(D4-ZN$+mg&7$u!! z-^+Z%;-3IDwqZ|K=ah85OLwkO zKxNBh+4QHh)u9D?MFtpbl)us}9+V!D%w9jfAMYEb>%$A;u)rrI zuBudh;5PN}_6J_}l55P3l_)&RMlH{m!)ai-i$g)&*M`eN$XQMw{v^r@-125^RRCF0 z^2>|DxhQw(mtNEI2Kj(;KblC7x=JlK$@78`O~>V!`|1Lm-^JR$-5pUANAnb(5}B}JGjBsliK4& zk6y(;$e&h)lh2)L=bvZKbvh@>vLlreBdH8No2>$#%_Wp1U0N7Ank!6$dFSi#xzh|( zRi{Uw%-4W!{IXZ)fWx@XX6;&(m_F%c6~X8hx=BN1&q}*( zoaNjWabE{oUPb!Bt$eyd#$5j9rItB-h*5JiNi(v^e|XKAj*8(k<5-2$&ZBR5fF|JA z9&m4fbzNQnAU}r8ab>fFV%J0z5awe#UZ|bz?Ur)U9bCIKWEzi2%A+5CLqh?}K4JHi z4vtM;+uPsVz{Lfr;78W78gC;z*yTch~4YkLr&m-7%-xc ztw6Mh2d>_iO*$Rd8(-Cr1_V8EO1f*^@wRoSozS) zy1UoC@pruAaC8Z_7~_w4Q6n*&B0AjOmMWa;sIav&gu z|J5&|{=a@vR!~k-OjKEgPFCzcJ>#A1uL&7xTDn;{XBdeM}V=l3B8fE1--DHjSaxoSjNKEM9|U9#m2<3>n{Iuo`r3UZp;>GkT2YBNAh|b z^jTq-hJp(ebZh#Lk8hVBP%qXwv-@vbvoREX$TqRGTgEi$%_F9tZES@z8Bx}$#5eeG zk^UsLBH{bc2VBW)*EdS({yw=?qmevwi?BL6*=12k9zM5gJv1>y#ML4!)iiPzVaH9% zgSImetD@dam~e>{LvVh!phhzpW+iFvWpGT#CVE5TQ40n%F|p(sP5mXxna+Ev7PDwA zamaV4m*^~*xV+&p;W749xhb_X=$|LD;FHuB&JL5?*Y2-oIT(wYY2;73<^#46S~Gx| z^cez%V7x$81}UWqS13Gz80379Rj;6~WdiXWOSsdmzY39L;Hg3MH43o*y8ibNBBH`(av4|u;YPq%{R;IuYow<+GEsf@R?=@tT@!}?#>zIIn0CoyV!hq3mw zHj>OOjfJM3F{RG#6ujzo?y32m^tgSXf@v=J$ELdJ+=5j|=F-~hP$G&}tDZsZE?5rX ztGj`!S>)CFmdkccxM9eGIcGnS2AfK#gXwj%esuIBNJQP1WV~b~+D7PJTmWGTSDrR` zEAu4B8l>NPuhsk5a`rReSya2nfV1EK01+G!x8aBdTs3Io$u5!6n6KX%uv@DxAp3F@{4UYg4SWJtQ-W~0MDb|j-$lwVn znAm*Pl!?Ps&3wO=R115RWKb*JKoexo*)uhhHBncEDMSVa_PyA>k{Zm2(wMQ(5NM3# z)jkza|GoWEQo4^s*wE(gHz?Xsg4`}HUAcs42cM1-qq_=+=!Gk^y710j=66(cSWqUe zklbm8+zB_syQv5A2rj!Vbw8;|$@C!vfNmNV!yJIWDQ>{+2x zKjuFX`~~HKG~^6h5FntRpnnHt=D&rq0>IJ9#F0eM)Y-)GpRjiN7gkA8wvnG#K=q{q z9dBn8_~wm4J<3J_vl|9H{7q6u2A!cW{bp#r*-f{gOV^e=8S{nc1DxMHFwuM$;aVI^ zz6A*}m8N-&x8;aunp1w7_vtB*pa+OYBw=TMc6QK=mbA-|Cf* zvyh8D4LRJImooUaSb7t*fVfih<97Gf@VE0|z>NcBwBQze);Rh!k3K_sfunToZY;f2 z^HmC4KjHRVg+eKYj;PRN^|E0>Gj_zagfRbrki68I^#~6-HaHg3BUW%+clM1xQEdPYt_g<2K+z!$>*$9nQ>; zf9Bei{?zY^-e{q_*|W#2rJG`2fy@{%6u0i_VEWTq$*(ZN37|8lFFFt)nCG({r!q#9 z5VK_kkSJ3?zOH)OezMT{!YkCuSSn!K#-Rhl$uUM(bq*jY? zi1xbMVthJ`E>d>(f3)~fozjg^@eheMF6<)I`oeJYx4*+M&%c9VArn(OM-wp%M<-`x z7sLP1&3^%Nld9Dhm@$3f2}87!quhI@nwd@3~fZl_3LYW-B?Ia>ui`ELg z&Qfe!7m6ze=mZ`Ia9$z|ARSw|IdMpooY4YiPN8K z4B(ts3p%2i(Td=tgEHX z0UQ_>URBtG+-?0E;E7Ld^dyZ;jjw0}XZ(}-QzC6+NN=40oDb2^v!L1g9xRvE#@IBR zO!b-2N7wVfLV;mhEaXQ9XAU+>=XVA6f&T4Z-@AX!leJ8obP^P^wP0aICND?~w&NykJ#54x3_@r7IDMdRNy4Hh;h*!u(Ol(#0bJdwEo$5437-UBjQ+j=Ic>Q2z` zJNDf0yO6@mr6y1#n3)s(W|$iE_i8r@Gd@!DWDqZ7J&~gAm1#~maIGJ1sls^gxL9LLG_NhU!pTGty!TbhzQnu)I*S^54U6Yu%ZeCg`R>Q zhBv$n5j0v%O_j{QYWG!R9W?5_b&67KB$t}&e2LdMvd(PxN6Ir!H4>PNlerpBL>Zvyy!yw z-SOo8caEpDt(}|gKPBd$qND5#a5nju^O>V&;f890?yEOfkSG^HQVmEbM3Ugzu+UtH zC(INPDdraBN?P%kE;*Ae%Wto&sgw(crfZ#Qy(<4nk;S|hD3j{IQRI6Yq|f^basLY; z-HB&Je%Gg}Jt@={_C{L$!RM;$$|iD6vu#3w?v?*;&()uB|I-XqEKqZPS!reW9JkLewLb!70T7n`i!gNtb1%vN- zySZj{8-1>6E%H&=V}LM#xmt`J3XQoaD|@XygXjdZ1+P77-=;=eYpoEQ01B@L*a(uW zrZeZz?HJsw_4g0vhUgkg@VF8<-X$B8pOqCuWAl28uB|@r`19DTUQQsb^pfqB6QtiT z*`_UZ`fT}vtUY#%sq2{rchyfu*pCg;uec2$-$N_xgjZcoumE5vSI{+s@iLWoz^Mf; zuI8kDP{!XY6OP~q5}%1&L}CtfH^N<3o4L@J@zg1-mt{9L`s^z$Vgb|mr{@WiwAqKg zp#t-lhrU>F8o0s1q_9y`gQNf~Vb!F%70f}$>i7o4ho$`uciNf=xgJ>&!gSt0g;M>*x4-`U)ysFW&Vs^Vk6m%?iuWU+o&m(2Jm26Y(3%TL; zA7T)BP{WS!&xmxNw%J=$MPfn(9*^*TV;$JwRy8Zl*yUZi8jWYF>==j~&S|Xinsb%c z2?B+kpet*muEW7@AzjBA^wAJBY8i|#C{WtO_or&Nj2{=6JTTX05}|H>N2B|Wf!*3_ z7hW*j6p3TvpghEc6-wufFiY!%-GvOx*bZrhZu+7?iSrZL5q9}igiF^*R3%DE4aCHZ zqu>xS8LkW+Auv%z-<1Xs92u23R$nk@Pk}MU5!gT|c7vGlEA%G^2th&Q*zfg%-D^=f z&J_}jskj|Q;73NP4<4k*Y%pXPU2Thoqr+5uH1yEYM|VtBPW6lXaetokD0u z9qVek6Q&wk)tFbQ8(^HGf3Wp16gKmr>G;#G(HRBx?F`9AIRboK+;OfHaLJ(P>IP0w zyTbTkx_THEOs%Q&aPrxbZrJlio+hCC_HK<4%f3ZoSAyG7Dn`=X=&h@m*|UYO-4Hq0 z-Bq&+Ie!S##4A6OGoC~>ZW`Y5J)*ouaFl_e9GA*VSL!O_@xGiBw!AF}1{tB)z(w%c zS1Hmrb9OC8>0a_$BzeiN?rkPLc9%&;1CZW*4}CDDNr2gcl_3z+WC15&H1Zc2{o~i) z)LLW=WQ{?ricmC`G1GfJ0Yp4Dy~Ba;j6ZV4r{8xRs`13{dD!xXmr^Aga|C=iSmor% z8hi|pTXH)5Yf&v~exp3o+sY4B^^b*eYkkCYl*T{*=-0HniSA_1F53eCb{x~1k3*`W zr~};p1A`k{1DV9=UPnLDgz{aJH=-LQo<5%+Em!DNN252xwIf*wF_zS^!(XSm(9eoj z=*dXG&n0>)_)N5oc6v!>-bd(2ragD8O=M|wGW z!xJQS<)u70m&6OmrF0WSsr@I%T*c#Qo#Ha4d3COcX+9}hM5!7JIGF>7<~C(Ear^Sn zm^ZFkV6~Ula6+8S?oOROOA6$C&q&dp`>oR-2Ym3(HT@O7Sd5c~+kjrmM)YmgPH*tL zX+znN>`tv;5eOfX?h{AuX^LK~V#gPCu=)Tigtq9&?7Xh$qN|%A$?V*v=&-2F$zTUv z`C#WyIrChS5|Kgm_GeudCFf;)!WH7FI60j^0o#65o6`w*S7R@)88n$1nrgU(oU0M9 zx+EuMkC>(4j1;m6NoGqEkpJYJ?vc|B zOlwT3t&UgL!pX_P*6g36`ZXQ; z9~Cv}ANFnJGp(;ZhS(@FT;3e)0)Kp;h^x;$*xZn*k0U6-&FwI=uOGaODdrsp-!K$Ac32^c{+FhI-HkYd5v=`PGsg%6I`4d9Jy)uW0y%) zm&j^9WBAp*P8#kGJUhB!L?a%h$hJgQrx!6KCB_TRo%9{t0J7KW8!o1B!NC)VGLM5! zpZy5Jc{`r{1e(jd%jsG7k%I+m#CGS*BPA65ZVW~fLYw0dA-H_}O zrkGFL&P1PG9p2(%QiEWm6x;U-U&I#;Em$nx-_I^wtgw3xUPVVu zqSuKnx&dIT-XT+T10p;yjo1Y)z(x1fb8Dzfn8e yu?e%!_ptzGB|8GrCfu%p?(_ zQccdaaVK$5bz;*rnyK{_SQYM>;aES6Qs^lj9lEs6_J+%nIiuQC*fN;z8md>r_~Mfl zU%p5Dt_YT>gQqfr@`cR!$NWr~+`CZb%dn;WtzrAOI>P_JtsB76PYe*<%H(y>qx-`Kq!X_; z<{RpAqYhE=L1r*M)gNF3B8r(<%8mo*SR2hu zccLRZwGARt)Hlo1euqTyM>^!HK*!Q2P;4UYrysje@;(<|$&%vQekbn|0Ruu_Io(w4#%p6ld2Yp7tlA`Y$cciThP zKzNGIMPXX%&Ud0uQh!uQZz|FB`4KGD?3!ND?wQt6!n*f4EmCoJUh&b?;B{|lxs#F- z31~HQ`SF4x$&v00@(P+j1pAaj5!s`)b2RDBp*PB=2IB>oBF!*6vwr7Dp%zpAx*dPr zb@Zjq^XjN?O4QcZ*O+8>)|HlrR>oD*?WQl5ri3R#2?*W6iJ>>kH%KnnME&TT@ZzrHS$Q%LC?n|e>V+D+8D zYc4)QddFz7I8#}y#Wj6>4P%34dZH~OUDb?uP%-E zwjXM(?Sg~1!|wI(RVuxbu)-rH+O=igSho_pDCw(c6b=P zKk4ATlB?bj9+HHlh<_!&z0rx13K3ZrAR8W)!@Y}o`?a*JJsD+twZIv`W)@Y?Amu_u zz``@-e2X}27$i(2=9rvIu5uTUOVhzwu%mNazS|lZb&PT;XE2|B&W1>=B58#*!~D&) zfVmJGg8UdP*fx(>Cj^?yS^zH#o-$Q-*$SnK(ZVFkw+er=>N^7!)FtP3y~Xxnu^nzY zikgB>Nj0%;WOltWIob|}%lo?_C7<``a5hEkx&1ku$|)i>Rh6@3h*`slY=9U}(Ql_< zaNG*J8vb&@zpdhAvv`?{=zDedJ23TD&Zg__snRAH4eh~^oawdYi6A3w8<Ozh@Kw)#bdktM^GVb zrG08?0bG?|NG+w^&JvD*7LAbjED{_Zkc`3H!My>0u5Q}m!+6VokMLXxl`Mkd=g&Xx z-a>m*#G3SLlhbKB!)tnzfWOBV;u;ftU}S!NdD5+YtOjLg?X}dl>7m^gOpihrf1;PY zvll&>dIuUGs{Qnd- zwIR3oIrct8Va^Tm0t#(bJD7c$Z7DO9*7NnRZorrSm`b`cxz>OIC;jSE3DO8`hX955ui`s%||YQtt2 z5DNA&pG-V+4oI2s*x^>-$6J?p=I>C|9wZF8z;VjR??Icg?1w2v5Me+FgAeGGa8(3S z4vg*$>zC-WIVZtJ7}o9{D-7d>zCe|z#<9>CFve-OPAYsneTb^JH!Enaza#j}^mXy1 z+ULn^10+rWLF6j2>Ya@@Kq?26>AqK{A_| zQKb*~F1>sE*=d?A?W7N2j?L09_7n+HGi{VY;MoTGr_)G9)ot$p!-UY5zZ2Xtbm=t z@dpPSGwgH=QtIcEulQNI>S-#ifbnO5EWkI;$A|pxJd885oM+ zGZ0_0gDvG8q2xebj+fbCHYfAXuZStH2j~|d^sBAzo46(K8n59+T6rzBwK)^rfPT+B zyIFw)9YC-V^rhtK`!3jrhmW-sTmM+tPH+;nwjL#-SjQPUZ53L@A>y*rt(#M(qsiB2 zx6B)dI}6Wlsw%bJ8h|(lhkJVogQZA&n{?Vgs6gNSXzuZpEyu*xySy8ro07QZ7Vk1!3tJphN_5V7qOiyK8p z#@jcDD8nmtYi1^l8ml;AF<#IPK?!pqf9D4moYk>d99Im}Jtwj6c#+A;f)CQ*f-hZ< z=p_T86jog%!p)D&5g9taSwYi&eP z#JuEK%+NULWus;0w32-SYFku#i}d~+{Pkho&^{;RxzP&0!RCm3-9K6`>KZpnzS6?L z^H^V*s!8<>x8bomvD%rh>Zp3>Db%kyin;qtl+jAv8Oo~1g~mqGAC&Qi_wy|xEt2iz zWAJEfTV%cl2Cs<1L&DLRVVH05EDq`pH7Oh7sR`NNkL%wi}8n>IXcO40hp+J+sC!W?!krJf!GJNE8uj zg-y~Ns-<~D?yqbzVRB}G>0A^f0!^N7l=$m0OdZuqAOQqLc zX?AEGr1Ht+inZ-Qiwnl@Z0qukd__a!C*CKuGdy5#nD7VUBM^6OCpxCa2A(X;e0&V4 zM&WR8+wErQ7UIc6LY~Q9x%Sn*Tn>>P`^t&idaOEnOd(Ufw#>NoR^1QdhJ8s`h^|R_ zXX`c5*O~Xdvh%q;7L!_!ohf$NfEBmCde|#uVZvEo>OfEq%+Ns7&_f$OR9xsihRpBb z+cjk8LyDm@U{YN>+r46?nn{7Gh(;WhFw6GAxtcKD+YWV?uge>;+q#Xx4!GpRkVZYu zzsF}1)7$?%s9g9CH=Zs+B%M_)+~*j3L0&Q9u7!|+T`^O{xE6qvAP?XWv9_MrZKdo& z%IyU)$Q95AB4!#hT!_dA>4e@zjOBD*Y=XjtMm)V|+IXzjuM;(l+8aA5#Kaz_$rR6! zj>#&^DidYD$nUY(D$mH`9eb|dtV0b{S>H6FBfq>t5`;OxA4Nn{J(+XihF(stSche7$es&~N$epi&PDM_N`As;*9D^L==2Q7Z2zD+CiU(|+-kL*VG+&9!Yb3LgPy?A zm7Z&^qRG_JIxK7-FBzZI3Q<;{`DIxtc48k> zc|0dmX;Z=W$+)qE)~`yn6MdoJ4co;%!`ddy+FV538Y)j(vg}5*k(WK)KWZ3WaOG!8 z!syGn=s{H$odtpqFrT#JGM*utN7B((abXnpDM6w56nhw}OY}0TiTG1#f*VFZr+^-g zbP10`$LPq_;PvrA1XXlyx2uM^mrjTzX}w{yuLo-cOClE8MMk47T25G8M!9Z5ypOSV zAJUBGEg5L2fY)ZGJb^E34R2zJ?}Vf>{~gB!8=5Z) z9y$>5c)=;o0HeHHSuE4U)#vG&KF|I%-cF6f$~pdYJWk_dD}iOA>iA$O$+4%@>JU08 zS`ep)$XLPJ+n0_i@PkF#ri6T8?ZeAot$6JIYHm&P6EB=BiaNY|aA$W0I+nz*zkz_z zkEru!tj!QUffq%)8y0y`T&`fuus-1p>=^hnBiBqD^hXrPs`PY9tU3m0np~rISY09> z`P3s=-kt_cYcxWd{de@}TwSqg*xVhp;E9zCsnXo6z z?f&Sv^U7n4`xr=mXle94HzOdN!2kB~4=%)u&N!+2;z6UYKUDqi-s6AZ!haB;@&B`? z_TRX0%@suz^TRdCb?!vNJYPY8L_}&07uySH9%W^Tc&1pia6y1q#?*Drf}GjGbPjBS zbOPcUY#*$3sL2x4v_i*Y=N7E$mR}J%|GUI(>WEr+28+V z%v5{#e!UF*6~G&%;l*q*$V?&r$Pp^sE^i-0$+RH3ERUUdQ0>rAq2(2QAbG}$y{de( z>{qD~GGuOk559Y@%$?N^1ApVL_a704>8OD%8Y%8B;FCt%AoPu8*D1 zLB5X>b}Syz81pn;xnB}%0FnwazlWfUV)Z-~rZg6~b z6!9J$EcE&sEbzcy?CI~=boWA&eeIa%z(7SE^qgVLz??1Vbc1*aRvc%Mri)AJaAG!p z$X!_9Ds;Zz)f+;%s&dRcJt2==P{^j3bf0M=nJd&xwUGlUFn?H=2W(*2I2Gdu zv!gYCwM10aeus)`RIZSrCK=&oKaO_Ry~D1B5!y0R=%!i2*KfXGYX&gNv_u+n9wiR5 z*e$Zjju&ODRW3phN925%S(jL+bCHv6rZtc?!*`1TyYXT6%Ju=|X;6D@lq$8T zW{Y|e39ioPez(pBH%k)HzFITXHvnD6hw^lIoUMA;qAJ^CU?top1fo@s7xT13Fvn1H z6JWa-6+FJF#x>~+A;D~;VDs26>^oH0EI`IYT2iagy23?nyJ==i{g4%HrAf1-*v zK1)~@&(KkwR7TL}L(A@C_S0G;-GMDy=MJn2$FP5s<%wC)4jC5PXoxrQBFZ_k0P{{s@sz+gX`-!=T8rcB(=7vW}^K6oLWMmp(rwDh}b zwaGGd>yEy6fHv%jM$yJXo5oMAQ>c9j`**}F?MCry;T@47@r?&sKHgVe$MCqk#Z_3S z1GZI~nOEN*P~+UaFGnj{{Jo@16`(qVNtbU>O0Hf57-P>x8Jikp=`s8xWs^dAJ9lCQ z)GFm+=OV%AMVqVATtN@|vp61VVAHRn87}%PC^RAzJ%JngmZTasWBAWsoAqBU+8L8u z4A&Pe?fmTm0?mK-BL9t+{y7o(7jm+RpOhL9KnY#E&qu^}B6=K_dB}*VlSEiC9fn)+V=J;OnN)Ta5v66ic1rG+dGAJ1 z1%Zb_+!$=tQ~lxQrzv3x#CPb?CekEkA}0MYSgx$Jdd}q8+R=ma$|&1a#)TQ=l$1tQ z=tL9&_^vJ)Pk}EDO-va`UCT1m#Uty1{v^A3P~83_#v^ozH}6*9mIjIr;t3Uv%@VeW zGL6(CwCUp)Jq%G0bIG%?{_*Y#5IHf*5M@wPo6A{$Um++Co$wLC=J1aoG93&T7Ho}P z=mGEPP7GbvoG!uD$k(H3A$Z))+i{Hy?QHdk>3xSBXR0j!11O^mEe9RHmw!pvzv?Ua~2_l2Yh~_!s1qS`|0~0)YsbHSz8!mG)WiJE| z2f($6TQtt6L_f~ApQYQKSb=`053LgrQq7G@98#igV>y#i==-nEjQ!XNu9 z~;mE+gtj4IDDNQJ~JVk5Ux6&LCSFL!y=>79kE9=V}J7tD==Ga+IW zX)r7>VZ9dY=V&}DR))xUoV!u(Z|%3ciQi_2jl}3=$Agc(`RPb z8kEBpvY>1FGQ9W$n>Cq=DIpski};nE)`p3IUw1Oz0|wxll^)4dq3;CCY@RyJgFgc# zKouFh!`?Xuo{IMz^xi-h=StCis_M7yq$u) z?XHvw*HP0VgR+KR6wI)jEMX|ssqYvSf*_3W8zVTQzD?3>H!#>InzpSO)@SC8q*ii- z%%h}_#0{4JG;Jm`4zg};BPTGkYamx$Xo#O~lBirRY)q=5M45n{GCfV7h9qwyu1NxOMoP4)jjZMxmT|IQQh0U7C$EbnMN<3)Kk?fFHYq$d|ICu>KbY_hO zTZM+uKHe(cIZfEqyzyYSUBZa8;Fcut-GN!HSA9ius`ltNebF46ZX_BbZNU}}ZOm{M2&nANL9@0qvih15(|`S~z}m&h!u4x~(%MAO$jHRWNfuxWF#B)E&g3ghSQ9|> z(MFaLQj)NE0lowyjvg8z0#m6FIuKE9lDO~Glg}nSb7`~^&#(Lw{}GVOS>U)m8bF}x zVjbXljBm34Cs-yM6TVusr+3kYFjr28STT3g056y3cH5Tmge~ASxBj z%|yb>$eF;WgrcOZf569sDZOVwoo%8>XO>XQOX1OyN9I-SQgrm;U;+#3OI(zrWyow3 zk==|{lt2xrQ%FIXOTejR>;wv(Pb8u8}BUpx?yd(Abh6? zsoO3VYWkeLnF43&@*#MQ9-i-d0t*xN-UEyNKeyNMHw|A(k(_6QKO=nKMCxD(W(Yop zsRQ)QeL4X3Lxp^L%wzi2-WVSsf61dqliPUM7srDB?Wm6Lzn0&{*}|IsKQW;02(Y&| zaTKv|`U(pSzuvR6Rduu$wzK_W-Y-7>7s?G$)U}&uK;<>vU}^^ns@Z!p+9?St1s)dG zK%y6xkPyyS1$~&6v{kl?Md6gwM|>mt6Upm>oa8RLD^8T{0?HC!Z>;(Bob7el(DV6x zi`I)$&E&ngwFS@bi4^xFLAn`=fzTC;aimE^!cMI2n@Vo%Ae-ne`RF((&5y6xsjjAZ zVguVoQ?Z9uk$2ON;ersE%PU*xGO@T*;j1BO5#TuZKEf(mB7|g7pcEA=nYJ{s3vlbg zd4-DUlD{*6o%Gc^N!Nptgay>j6E5;3psI+C3Q!1ZIbeCubW%w4pq9)MSDyB{HLm|k zxv-{$$A*pS@csolri$Ge<4VZ}e~78JOL-EVyrbxKra^d{?|NnPp86!q>t<&IP07?Z z^>~IK^k#OEKgRH+LjllZXk7iA>2cfH6+(e&9ku5poo~6y{GC5>(bRK7hwjiurqAiZ zg*DmtgY}v83IjE&AbiWgMyFbaRUPZ{lYiz$U^&Zt2YjG<%m((&_JUbZcfJ22(>bi5 z!J?<7AySj0JZ&<-qXX;mcV!f~>G=sB0KnjWca4}vrtunD^1TrpfeS^4dvFr!65knK zZh`d;*VOkPs4*-9kL>$GP0`(M!j~B;#x?Ba~&s6CopvO86oM?-? zOw#dIRc;6A6T?B`Qp%^<U5 z19x(ywSH$_N+Io!6;e?`tWaM$`=Db!gzx|lQ${DG!zb1Zl&|{kX0y6xvO1o z220r<-oaS^^R2pEyY;=Qllqpmue|5yI~D|iI!IGt@iod{Opz@*ml^w2bNs)p`M(Io z|E;;m*Xpjd9l)4G#KaWfV(t8YUn@A;nK^#xgv=LtnArX|vWQVuw3}B${h+frU2>9^ z!l6)!Uo4`5k`<<;E(ido7M6lKTgWezNLq>U*=uz&s=cc$1%>VrAeOoUtA|T6gO4>UNqsdK=NF*8|~*sl&wI=x9-EGiq*aqV!(VVXA57 zw9*o6Ir8Lj1npUXvlevtn(_+^X5rzdR>#(}4YcB9O50q97%rW2me5_L=%ffYPUSRc z!vv?Kv>dH994Qi>U(a<0KF6NH5b16enCp+mw^Hb3Xs1^tThFpz!3QuN#}KBbww`(h z7GO)1olDqy6?T$()R7y%NYx*B0k_2IBiZ14&8|JPFxeMF{vW>HF-Vi3+ZOI=+qP}n zw(+!WcTd~4ZJX1!ZM&y!+uyt=&i!+~d(V%GjH;-NsEEv6nS1TERt|RHh!0>W4+4pp z1-*EzAM~i`+1f(VEHI8So`S`akPfPTfq*`l{Fz`hS%k#JS0cjT2mS0#QLGf=J?1`he3W*;m4)ce8*WFq1sdP=~$5RlH1EdWm|~dCvKOi4*I_96{^95p#B<(n!d?B z=o`0{t+&OMwKcxiBECznJcfH!fL(z3OvmxP#oWd48|mMjpE||zdiTBdWelj8&Qosv zZFp@&UgXuvJw5y=q6*28AtxZzo-UUpkRW%ne+Ylf!V-0+uQXBW=5S1o#6LXNtY5!I z%Rkz#(S8Pjz*P7bqB6L|M#Er{|QLae-Y{KA>`^} z@lPjeX>90X|34S-7}ZVXe{wEei1<{*e8T-Nbj8JmD4iwcE+Hg_zhkPVm#=@b$;)h6 z<<6y`nPa`f3I6`!28d@kdM{uJOgM%`EvlQ5B2bL)Sl=|y@YB3KeOzz=9cUW3clPAU z^sYc}xf9{4Oj?L5MOlYxR{+>w=vJjvbyO5}ptT(o6dR|ygO$)nVCvNGnq(6;bHlBd zl?w-|plD8spjDF03g5ip;W3Z z><0{BCq!Dw;h5~#1BuQilq*TwEu)qy50@+BE4bX28+7erX{BD4H)N+7U`AVEuREE8 z;X?~fyhF-x_sRfHIj~6f(+^@H)D=ngP;mwJjxhQUbUdzk8f94Ab%59-eRIq?ZKrwD z(BFI=)xrUlgu(b|hAysqK<}8bslmNNeD=#JW*}^~Nrswn^xw*nL@Tx!49bfJecV&KC2G4q5a!NSv)06A_5N3Y?veAz;Gv+@U3R% z)~UA8-0LvVE{}8LVDOHzp~2twReqf}ODIyXMM6=W>kL|OHcx9P%+aJGYi_Om)b!xe zF40Vntn0+VP>o<$AtP&JANjXBn7$}C@{+@3I@cqlwR2MdwGhVPxlTIcRVu@Ho-wO` z_~Or~IMG)A_`6-p)KPS@cT9mu9RGA>dVh5wY$NM9-^c@N=hcNaw4ITjm;iWSP^ZX| z)_XpaI61<+La+U&&%2a z0za$)-wZP@mwSELo#3!PGTt$uy0C(nTT@9NX*r3Ctw6J~7A(m#8fE)0RBd`TdKfAT zCf@$MAxjP`O(u9s@c0Fd@|}UQ6qp)O5Q5DPCeE6mSIh|Rj{$cAVIWsA=xPKVKxdhg zLzPZ`3CS+KIO;T}0Ip!fAUaNU>++ZJZRk@I(h<)RsJUhZ&Ru9*!4Ptn;gX^~4E8W^TSR&~3BAZc#HquXn)OW|TJ`CTahk+{qe`5+ixON^zA9IFd8)kc%*!AiLu z>`SFoZ5bW-%7}xZ>gpJcx_hpF$2l+533{gW{a7ce^B9sIdmLrI0)4yivZ^(Vh@-1q zFT!NQK$Iz^xu%|EOK=n>ug;(7J4OnS$;yWmq>A;hsD_0oAbLYhW^1Vdt9>;(JIYjf zdb+&f&D4@4AS?!*XpH>8egQvSVX`36jMd>$+RgI|pEg))^djhGSo&#lhS~9%NuWfX zDDH;3T*GzRT@5=7ibO>N-6_XPBYxno@mD_3I#rDD?iADxX`! zh*v8^i*JEMzyN#bGEBz7;UYXki*Xr(9xXax(_1qVW=Ml)kSuvK$coq2A(5ZGhs_pF z$*w}FbN6+QDseuB9=fdp_MTs)nQf!2SlROQ!gBJBCXD&@-VurqHj0wm@LWX-TDmS= z71M__vAok|@!qgi#H&H%Vg-((ZfxPAL8AI{x|VV!9)ZE}_l>iWk8UPTGHs*?u7RfP z5MC&=c6X;XlUzrz5q?(!eO@~* zoh2I*%J7dF!!_!vXoSIn5o|wj1#_>K*&CIn{qSaRc&iFVxt*^20ngCL;QonIS>I5^ zMw8HXm>W0PGd*}Ko)f|~dDd%;Wu_RWI_d;&2g6R3S63Uzjd7dn%Svu-OKpx*o|N>F zZg=-~qLb~VRLpv`k zWSdfHh@?dp=s_X`{yxOlxE$4iuyS;Z-x!*E6eqmEm*j2bE@=ZI0YZ5%Yj29!5+J$4h{s($nakA`xgbO8w zi=*r}PWz#lTL_DSAu1?f%-2OjD}NHXp4pXOsCW;DS@BC3h-q4_l`<))8WgzkdXg3! zs1WMt32kS2E#L0p_|x+x**TFV=gn`m9BWlzF{b%6j-odf4{7a4y4Uaef@YaeuPhU8 zHBvRqN^;$Jizy+ z=zW{E5<>2gp$pH{M@S*!sJVQU)b*J5*bX4h>5VJve#Q6ga}cQ&iL#=(u+KroWrxa%8&~p{WEUF0il=db;-$=A;&9M{Rq`ouZ5m%BHT6%st%saGsD6)fQgLN}x@d3q>FC;=f%O3Cyg=Ke@Gh`XW za@RajqOE9UB6eE=zhG%|dYS)IW)&y&Id2n7r)6p_)vlRP7NJL(x4UbhlcFXWT8?K=%s7;z?Vjts?y2+r|uk8Wt(DM*73^W%pAkZa1Jd zNoE)8FvQA>Z`eR5Z@Ig6kS5?0h;`Y&OL2D&xnnAUzQz{YSdh0k zB3exx%A2TyI)M*EM6htrxSlep!Kk(P(VP`$p0G~f$smld6W1r_Z+o?=IB@^weq>5VYsYZZR@` z&XJFxd5{|KPZmVOSxc@^%71C@;z}}WhbF9p!%yLj3j%YOlPL5s>7I3vj25 z@xmf=*z%Wb4;Va6SDk9cv|r*lhZ`(y_*M@>q;wrn)oQx%B(2A$9(74>;$zmQ!4fN; z>XurIk-7@wZys<+7XL@0Fhe-f%*=(weaQEdR9Eh6>Kl-EcI({qoZqyzziGwpg-GM#251sK_ z=3|kitS!j%;fpc@oWn65SEL73^N&t>Ix37xgs= zYG%eQDJc|rqHFia0!_sm7`@lvcv)gfy(+KXA@E{3t1DaZ$DijWAcA)E0@X?2ziJ{v z&KOYZ|DdkM{}t+@{@*6ge}m%xfjIxi%qh`=^2Rwz@w0cCvZ&Tc#UmCDbVwABrON^x zEBK43FO@weA8s7zggCOWhMvGGE`baZ62cC)VHyy!5Zbt%ieH+XN|OLbAFPZWyC6)p z4P3%8sq9HdS3=ih^0OOlqTPbKuzQ?lBEI{w^ReUO{V?@`ARsL|S*%yOS=Z%sF)>-y z(LAQdhgAcuF6LQjRYfdbD1g4o%tV4EiK&ElLB&^VZHbrV1K>tHTO{#XTo>)2UMm`2 z^t4s;vnMQgf-njU-RVBRw0P0-m#d-u`(kq7NL&2T)TjI_@iKuPAK-@oH(J8?%(e!0Ir$yG32@CGUPn5w4)+9@8c&pGx z+K3GKESI4*`tYlmMHt@br;jBWTei&(a=iYslc^c#RU3Q&sYp zSG){)V<(g7+8W!Wxeb5zJb4XE{I|&Y4UrFWr%LHkdQ;~XU zgy^dH-Z3lmY+0G~?DrC_S4@=>0oM8Isw%g(id10gWkoz2Q%7W$bFk@mIzTCcIB(K8 zc<5h&ZzCdT=9n-D>&a8vl+=ZF*`uTvQviG_bLde*k>{^)&0o*b05x$MO3gVLUx`xZ z43j+>!u?XV)Yp@MmG%Y`+COH2?nQcMrQ%k~6#O%PeD_WvFO~Kct za4XoCM_X!c5vhRkIdV=xUB3xI2NNStK*8_Zl!cFjOvp-AY=D;5{uXj}GV{LK1~IE2 z|KffUiBaStRr;10R~K2VVtf{TzM7FaPm;Y(zQjILn+tIPSrJh&EMf6evaBKIvi42-WYU9Vhj~3< zZSM-B;E`g_o8_XTM9IzEL=9Lb^SPhe(f(-`Yh=X6O7+6ALXnTcUFpI>ekl6v)ZQeNCg2 z^H|{SKXHU*%nBQ@I3It0m^h+6tvI@FS=MYS$ZpBaG7j#V@P2ZuYySbp@hA# ze(kc;P4i_-_UDP?%<6>%tTRih6VBgScKU^BV6Aoeg6Uh(W^#J^V$Xo^4#Ekp ztqQVK^g9gKMTHvV7nb64UU7p~!B?>Y0oFH5T7#BSW#YfSB@5PtE~#SCCg3p^o=NkMk$<8- z6PT*yIKGrvne7+y3}_!AC8NNeI?iTY(&nakN>>U-zT0wzZf-RuyZk^X9H-DT_*wk= z;&0}6LsGtfVa1q)CEUPlx#(ED@-?H<1_FrHU#z5^P3lEB|qsxEyn%FOpjx z3S?~gvoXy~L(Q{Jh6*i~=f%9kM1>RGjBzQh_SaIDfSU_9!<>*Pm>l)cJD@wlyxpBV z4Fmhc2q=R_wHCEK69<*wG%}mgD1=FHi4h!98B-*vMu4ZGW~%IrYSLGU{^TuseqVgV zLP<%wirIL`VLyJv9XG_p8w@Q4HzNt-o;U@Au{7%Ji;53!7V8Rv0^Lu^Vf*sL>R(;c zQG_ZuFl)Mh-xEIkGu}?_(HwkB2jS;HdPLSxVU&Jxy9*XRG~^HY(f0g8Q}iqnVmgjI zfd=``2&8GsycjR?M%(zMjn;tn9agcq;&rR!Hp z$B*gzHsQ~aXw8c|a(L^LW(|`yGc!qOnV(ZjU_Q-4z1&0;jG&vAKuNG=F|H?@m5^N@ zq{E!1n;)kNTJ>|Hb2ODt-7U~-MOIFo%9I)_@7fnX+eMMNh>)V$IXesJpBn|uo8f~#aOFytCT zf9&%MCLf8mp4kwHTcojWmM3LU=#|{3L>E}SKwOd?%{HogCZ_Z1BSA}P#O(%H$;z7XyJ^sjGX;j5 zrzp>|Ud;*&VAU3x#f{CKwY7Vc{%TKKqmB@oTHA9;>?!nvMA;8+Jh=cambHz#J18x~ zs!dF>$*AnsQ{{82r5Aw&^7eRCdvcgyxH?*DV5(I$qXh^zS>us*I66_MbL8y4d3ULj z{S(ipo+T3Ag!+5`NU2sc+@*m{_X|&p#O-SAqF&g_n7ObB82~$p%fXA5GLHMC+#qqL zdt`sJC&6C2)=juQ_!NeD>U8lDVpAOkW*khf7MCcs$A(wiIl#B9HM%~GtQ^}yBPjT@ z+E=|A!Z?A(rwzZ;T}o6pOVqHzTr*i;Wrc%&36kc@jXq~+w8kVrs;%=IFdACoLAcCAmhFNpbP8;s`zG|HC2Gv?I~w4ITy=g$`0qMQdkijLSOtX6xW%Z9Nw<;M- zMN`c7=$QxN00DiSjbVt9Mi6-pjv*j(_8PyV-il8Q-&TwBwH1gz1uoxs6~uU}PrgWB zIAE_I-a1EqlIaGQNbcp@iI8W1sm9fBBNOk(k&iLBe%MCo#?xI$%ZmGA?=)M9D=0t7 zc)Q0LnI)kCy{`jCGy9lYX%mUsDWwsY`;jE(;Us@gmWPqjmXL+Hu#^;k%eT>{nMtzj zsV`Iy6leTA8-PndszF;N^X@CJrTw5IIm!GPeu)H2#FQitR{1p;MasQVAG3*+=9FYK zw*k!HT(YQorfQj+1*mCV458(T5=fH`um$gS38hw(OqVMyunQ;rW5aPbF##A3fGH6h z@W)i9Uff?qz`YbK4c}JzQpuxuE3pcQO)%xBRZp{zJ^-*|oryTxJ-rR+MXJ)!f=+pp z10H|DdGd2exhi+hftcYbM0_}C0ZI-2vh+$fU1acsB-YXid7O|=9L!3e@$H*6?G*Zp z%qFB(sgl=FcC=E4CYGp4CN>=M8#5r!RU!u+FJVlH6=gI5xHVD&k;Ta*M28BsxfMV~ zLz+@6TxnfLhF@5=yQo^1&S}cmTN@m!7*c6z;}~*!hNBjuE>NLVl2EwN!F+)0$R1S! zR|lF%n!9fkZ@gPW|x|B={V6x3`=jS*$Pu0+5OWf?wnIy>Y1MbbGSncpKO0qE(qO=ts z!~@&!N`10S593pVQu4FzpOh!tvg}p%zCU(aV5=~K#bKi zHdJ1>tQSrhW%KOky;iW+O_n;`l9~omqM%sdxdLtI`TrJzN6BQz+7xOl*rM>xVI2~# z)7FJ^Dc{DC<%~VS?@WXzuOG$YPLC;>#vUJ^MmtbSL`_yXtNKa$Hk+l-c!aC7gn(Cg ze?YPYZ(2Jw{SF6MiO5(%_pTo7j@&DHNW`|lD`~{iH+_eSTS&OC*2WTT*a`?|9w1dh zh1nh@$a}T#WE5$7Od~NvSEU)T(W$p$s5fe^GpG+7fdJ9=enRT9$wEk+ZaB>G3$KQO zgq?-rZZnIv!p#>Ty~}c*Lb_jxJg$eGM*XwHUwuQ|o^}b3^T6Bxx{!?va8aC@-xK*H ztJBFvFfsSWu89%@b^l3-B~O!CXs)I6Y}y#0C0U0R0WG zybjroj$io0j}3%P7zADXOwHwafT#uu*zfM!oD$6aJx7+WL%t-@6^rD_a_M?S^>c;z zMK580bZXo1f*L$CuMeM4Mp!;P@}b~$cd(s5*q~FP+NHSq;nw3fbWyH)i2)-;gQl{S zZO!T}A}fC}vUdskGSq&{`oxt~0i?0xhr6I47_tBc`fqaSrMOzR4>0H^;A zF)hX1nfHs)%Zb-(YGX;=#2R6C{BG;k=?FfP?9{_uFLri~-~AJ;jw({4MU7e*d)?P@ zXX*GkNY9ItFjhwgAIWq7Y!ksbMzfqpG)IrqKx9q{zu%Mdl+{Dis#p9q`02pr1LG8R z@As?eG!>IoROgS!@J*to<27coFc1zpkh?w=)h9CbYe%^Q!Ui46Y*HO0mr% zEff-*$ndMNw}H2a5@BsGj5oFfd!T(F&0$<{GO!Qdd?McKkorh=5{EIjDTHU`So>8V zBA-fqVLb2;u7UhDV1xMI?y>fe3~4urv3%PX)lDw+HYa;HFkaLqi4c~VtCm&Ca+9C~ zge+67hp#R9`+Euq59WhHX&7~RlXn=--m8$iZ~~1C8cv^2(qO#X0?vl91gzUKBeR1J z^p4!!&7)3#@@X&2aF2-)1Ffcc^F8r|RtdL2X%HgN&XU-KH2SLCbpw?J5xJ*!F-ypZ zMG%AJ!Pr&}`LW?E!K~=(NJxuSVTRCGJ$2a*Ao=uUDSys!OFYu!Vs2IT;xQ6EubLIl z+?+nMGeQQhh~??0!s4iQ#gm3!BpMpnY?04kK375e((Uc7B3RMj;wE?BCoQGu=UlZt!EZ1Q*auI)dj3Jj{Ujgt zW5hd~-HWBLI_3HuO) zNrb^XzPsTIb=*a69wAAA3J6AAZZ1VsYbIG}a`=d6?PjM)3EPaDpW2YP$|GrBX{q*! z$KBHNif)OKMBCFP5>!1d=DK>8u+Upm-{hj5o|Wn$vh1&K!lVfDB&47lw$tJ?d5|=B z^(_9=(1T3Fte)z^>|3**n}mIX;mMN5v2F#l(q*CvU{Ga`@VMp#%rQkDBy7kYbmb-q z<5!4iuB#Q_lLZ8}h|hPODI^U6`gzLJre9u3k3c#%86IKI*^H-@I48Bi*@avYm4v!n0+v zWu{M{&F8#p9cx+gF0yTB_<2QUrjMPo9*7^-uP#~gGW~y3nfPAoV%amgr>PSyVAd@l)}8#X zR5zV6t*uKJZL}?NYvPVK6J0v4iVpwiN|>+t3aYiZSp;m0!(1`bHO}TEtWR1tY%BPB z(W!0DmXbZAsT$iC13p4f>u*ZAy@JoLAkJhzFf1#4;#1deO8#8d&89}en&z!W&A3++^1(;>0SB1*54d@y&9Pn;^IAf3GiXbfT`_>{R+Xv; zQvgL>+0#8-laO!j#-WB~(I>l0NCMt_;@Gp_f0#^c)t?&#Xh1-7RR0@zPyBz!U#0Av zT?}n({(p?p7!4S2ZBw)#KdCG)uPnZe+U|0{BW!m)9 zi_9$F?m<`2!`JNFv+w8MK_K)qJ^aO@7-Ig>cM4-r0bi=>?B_2mFNJ}aE3<+QCzRr*NA!QjHw# z`1OsvcoD0?%jq{*7b!l|L1+Tw0TTAM4XMq7*ntc-Ived>Sj_ZtS|uVdpfg1_I9knY z2{GM_j5sDC7(W&}#s{jqbybqJWyn?{PW*&cQIU|*v8YGOKKlGl@?c#TCnmnAkAzV- zmK={|1G90zz=YUvC}+fMqts0d4vgA%t6Jhjv?d;(Z}(Ep8fTZfHA9``fdUHkA+z3+ zhh{ohP%Bj?T~{i0sYCQ}uC#5BwN`skI7`|c%kqkyWIQ;!ysvA8H`b-t()n6>GJj6xlYDu~8qX{AFo$Cm3d|XFL=4uvc?Keb zzb0ZmMoXca6Mob>JqkNuoP>B2Z>D`Q(TvrG6m`j}-1rGP!g|qoL=$FVQYxJQjFn33lODt3Wb1j8VR zlR++vIT6^DtYxAv_hxupbLLN3e0%A%a+hWTKDV3!Fjr^cWJ{scsAdfhpI)`Bms^M6 zQG$waKgFr=c|p9Piug=fcJvZ1ThMnNhQvBAg-8~b1?6wL*WyqXhtj^g(Ke}mEfZVM zJuLNTUVh#WsE*a6uqiz`b#9ZYg3+2%=C(6AvZGc=u&<6??!slB1a9K)=VL zY9EL^mfyKnD zSJyYBc_>G;5RRnrNgzJz#Rkn3S1`mZgO`(r5;Hw6MveN(URf_XS-r58Cn80K)ArH4 z#Rrd~LG1W&@ttw85cjp8xV&>$b%nSXH_*W}7Ch2pg$$c0BdEo-HWRTZcxngIBJad> z;C>b{jIXjb_9Jis?NZJsdm^EG}e*pR&DAy0EaSGi3XWTa(>C%tz1n$u?5Fb z1qtl?;_yjYo)(gB^iQq?=jusF%kywm?CJP~zEHi0NbZ);$(H$w(Hy@{i>$wcVRD_X|w-~(0Z9BJyh zhNh;+eQ9BEIs;tPz%jSVnfCP!3L&9YtEP;svoj_bNzeGSQIAjd zBss@A;)R^WAu-37RQrM%{DfBNRx>v!G31Z}8-El9IOJlb_MSoMu2}GDYycNaf>uny z+8xykD-7ONCM!APry_Lw6-yT>5!tR}W;W`C)1>pxSs5o1z#j7%m=&=7O4hz+Lsqm` z*>{+xsabZPr&X=}G@obTb{nPTkccJX8w3CG7X+1+t{JcMabv~UNv+G?txRqXib~c^Mo}`q{$`;EBNJ;#F*{gvS12kV?AZ%O0SFB$^ zn+}!HbmEj}w{Vq(G)OGAzH}R~kS^;(-s&=ectz8vN!_)Yl$$U@HNTI-pV`LSj7Opu zTZ5zZ)-S_{GcEQPIQXLQ#oMS`HPu{`SQiAZ)m1at*Hy%3xma|>o`h%E%8BEbi9p0r zVjcsh<{NBKQ4eKlXU|}@XJ#@uQw*$4BxKn6#W~I4T<^f99~(=}a`&3(ur8R9t+|AQ zWkQx7l}wa48-jO@ft2h+7qn%SJtL%~890FG0s5g*kNbL3I&@brh&f6)TlM`K^(bhr zJWM6N6x3flOw$@|C@kPi7yP&SP?bzP-E|HSXQXG>7gk|R9BTj`e=4de9C6+H7H7n# z#GJeVs1mtHhLDmVO?LkYRQc`DVOJ_vdl8VUihO-j#t=0T3%Fc1f9F73ufJz*adn*p zc%&vi(4NqHu^R>sAT_0EDjVR8bc%wTz#$;%NU-kbDyL_dg0%TFafZwZ?5KZpcuaO54Z9hX zD$u>q!-9`U6-D`E#`W~fIfiIF5_m6{fvM)b1NG3xf4Auw;Go~Fu7cth#DlUn{@~yu z=B;RT*dp?bO}o%4x7k9v{r=Y@^YQ^UUm(Qmliw8brO^=NP+UOohLYiaEB3^DB56&V zK?4jV61B|1Uj_5fBKW;8LdwOFZKWp)g{B%7g1~DgO&N& z#lisxf?R~Z@?3E$Mms$$JK8oe@X`5m98V*aV6Ua}8Xs2#A!{x?IP|N(%nxsH?^c{& z@vY&R1QmQs83BW28qAmJfS7MYi=h(YK??@EhjL-t*5W!p z^gYX!Q6-vBqcv~ruw@oMaU&qp0Fb(dbVzm5xJN%0o_^@fWq$oa3X?9s%+b)x4w-q5Koe(@j6Ez7V@~NRFvd zfBH~)U5!ix3isg`6be__wBJp=1@yfsCMw1C@y+9WYD9_C%{Q~7^0AF2KFryfLlUP# zwrtJEcH)jm48!6tUcxiurAMaiD04C&tPe6DI0#aoqz#Bt0_7_*X*TsF7u*zv(iEfA z;$@?XVu~oX#1YXtceQL{dSneL&*nDug^OW$DSLF0M1Im|sSX8R26&)<0Fbh^*l6!5wfSu8MpMoh=2l z^^0Sr$UpZp*9oqa23fcCfm7`ya2<4wzJ`Axt7e4jJrRFVf?nY~2&tRL* zd;6_njcz01c>$IvN=?K}9ie%Z(BO@JG2J}fT#BJQ+f5LFSgup7i!xWRKw6)iITjZU z%l6hPZia>R!`aZjwCp}I zg)%20;}f+&@t;(%5;RHL>K_&7MH^S+7<|(SZH!u zznW|jz$uA`P9@ZWtJgv$EFp>)K&Gt+4C6#*khZQXS*S~6N%JDT$r`aJDs9|uXWdbg zBwho$phWx}x!qy8&}6y5Vr$G{yGSE*r$^r{}pw zVTZKvikRZ`J_IJrjc=X1uw?estdwm&bEahku&D04HD+0Bm~q#YGS6gp!KLf$A{%Qd z&&yX@Hp>~(wU{|(#U&Bf92+1i&Q*-S+=y=3pSZy$#8Uc$#7oiJUuO{cE6=tsPhwPe| zxQpK>`Dbka`V)$}e6_OXKLB%i76~4N*zA?X+PrhH<&)}prET;kel24kW%+9))G^JI zsq7L{P}^#QsZViX%KgxBvEugr>ZmFqe^oAg?{EI=&_O#e)F3V#rc z8$4}0Zr19qd3tE4#$3_f=Bbx9oV6VO!d3(R===i-7p=Vj`520w0D3W6lQfY48}!D* z&)lZMG;~er2qBoI2gsX+Ts-hnpS~NYRDtPd^FPzn!^&yxRy#CSz(b&E*tL|jIkq|l zf%>)7Dtu>jCf`-7R#*GhGn4FkYf;B$+9IxmqH|lf6$4irg{0ept__%)V*R_OK=T06 zyT_m-o@Kp6U{l5h>W1hGq*X#8*y@<;vsOFqEjTQXFEotR+{3}ODDnj;o0@!bB5x=N z394FojuGOtVKBlVRLtHp%EJv_G5q=AgF)SKyRN5=cGBjDWv4LDn$IL`*=~J7u&Dy5 zrMc83y+w^F&{?X(KOOAl-sWZDb{9X9#jrQtmrEXD?;h-}SYT7yM(X_6qksM=K_a;Z z3u0qT0TtaNvDER_8x*rxXw&C^|h{P1qxK|@pS7vdlZ#P z7PdB7MmC2}%sdzAxt>;WM1s0??`1983O4nFK|hVAbHcZ3x{PzytQLkCVk7hA!Lo` zEJH?4qw|}WH{dc4z%aB=0XqsFW?^p=X}4xnCJXK%c#ItOSjdSO`UXJyuc8bh^Cf}8 z@Ht|vXd^6{Fgai8*tmyRGmD_s_nv~r^Fy7j`Bu`6=G)5H$i7Q7lvQnmea&TGvJp9a|qOrUymZ$6G|Ly z#zOCg++$3iB$!6!>215A4!iryregKuUT344X)jQb3|9qY>c0LO{6Vby05n~VFzd?q zgGZv&FGlkiH*`fTurp>B8v&nSxNz)=5IF$=@rgND4d`!AaaX;_lK~)-U8la_Wa8i?NJC@BURO*sUW)E9oyv3RG^YGfN%BmxzjlT)bp*$<| zX3tt?EAy<&K+bhIuMs-g#=d1}N_?isY)6Ay$mDOKRh z4v1asEGWoAp=srraLW^h&_Uw|6O+r;wns=uwYm=JN4Q!quD8SQRSeEcGh|Eb5Jg8m zOT}u;N|x@aq)=&;wufCc^#)5U^VcZw;d_wwaoh9$p@Xrc{DD6GZUqZ ziC6OT^zSq@-lhbgR8B+e;7_Giv;DK5gn^$bs<6~SUadiosfewWDJu`XsBfOd1|p=q zE>m=zF}!lObA%ePey~gqU8S6h-^J2Y?>7)L2+%8kV}Gp=h`Xm_}rlm)SyUS=`=S7msKu zC|T!gPiI1rWGb1z$Md?0YJQ;%>uPLOXf1Z>N~`~JHJ!^@D5kSXQ4ugnFZ>^`zH8CAiZmp z6Ms|#2gcGsQ{{u7+Nb9sA?U>(0e$5V1|WVwY`Kn)rsnnZ4=1u=7u!4WexZD^IQ1Jk zfF#NLe>W$3m&C^ULjdw+5|)-BSHwpegdyt9NYC{3@QtMfd8GrIWDu`gd0nv-3LpGCh@wgBaG z176tikL!_NXM+Bv#7q^cyn9$XSeZR6#!B4JE@GVH zoobHZN_*RF#@_SVYKkQ_igme-Y5U}cV(hkR#k1c{bQNMji zU7aE`?dHyx=1`kOYZo_8U7?3-7vHOp`Qe%Z*i+FX!s?6huNp0iCEW-Z7E&jRWmUW_ z67j>)Ew!yq)hhG4o?^z}HWH-e=es#xJUhDRc4B51M4~E-l5VZ!&zQq`gWe`?}#b~7w1LH4Xa-UCT5LXkXQWheBa2YJYbyQ zl1pXR%b(KCXMO0OsXgl0P0Og<{(@&z1aokU-Pq`eQq*JYgt8xdFQ6S z6Z3IFSua8W&M#`~*L#r>Jfd6*BzJ?JFdBR#bDv$_0N!_5vnmo@!>vULcDm`MFU823 zpG9pqjqz^FE5zMDoGqhs5OMmC{Y3iVcl>F}5Rs24Y5B^mYQ;1T&ks@pIApHOdrzXF z-SdX}Hf{X;TaSxG_T$0~#RhqKISGKNK47}0*x&nRIPtmdwxc&QT3$8&!3fWu1eZ_P zJveQj^hJL#Sn!*4k`3}(d(aasl&7G0j0-*_2xtAnoX1@9+h zO#c>YQg60Z;o{Bi=3i7S`Ic+ZE>K{(u|#)9y}q*j8uKQ1^>+(BI}m%1v3$=4ojGBc zm+o1*!T&b}-lVvZqIUBc8V}QyFEgm#oyIuC{8WqUNV{Toz`oxhYpP!_p2oHHh5P@iB*NVo~2=GQm+8Yrkm2Xjc_VyHg1c0>+o~@>*Qzo zHVBJS>$$}$_4EniTI;b1WShX<5-p#TPB&!;lP!lBVBbLOOxh6FuYloD%m;n{r|;MU3!q4AVkua~fieeWu2 zQAQ$ue(IklX6+V;F1vCu-&V?I3d42FgWgsb_e^29ol}HYft?{SLf>DrmOp9o!t>I^ zY7fBCk+E8n_|apgM|-;^=#B?6RnFKlN`oR)`e$+;D=yO-(U^jV;rft^G_zl`n7qnM zL z*-Y4Phq+ZI1$j$F-f;`CD#|`-T~OM5Q>x}a>B~Gb3-+9i>Lfr|Ca6S^8g*{*?_5!x zH_N!SoRP=gX1?)q%>QTY!r77e2j9W(I!uAz{T`NdNmPBBUzi2{`XMB^zJGGwFWeA9 z{fk33#*9SO0)DjROug+(M)I-pKA!CX;IY(#gE!UxXVsa)X!UftIN98{pt#4MJHOhY zM$_l}-TJlxY?LS6Nuz1T<44m<4i^8k@D$zuCPrkmz@sdv+{ciyFJG2Zwy&%c7;atIeTdh!a(R^QXnu1Oq1b42*OQFWnyQ zWeQrdvP|w_idy53Wa<{QH^lFmEd+VlJkyiC>6B#s)F;w-{c;aKIm;Kp50HnA-o3lY z9B~F$gJ@yYE#g#X&3ADx&tO+P_@mnQTz9gv30_sTsaGXkfNYXY{$(>*PEN3QL>I!k zp)KibPhrfX3%Z$H6SY`rXGYS~143wZrG2;=FLj50+VM6soI~up_>fU(2Wl@{BRsMi zO%sL3x?2l1cXTF)k&moNsHfQrQ+wu(gBt{sk#CU=UhrvJIncy@tJX5klLjgMn>~h= zg|FR&;@eh|C7`>s_9c~0-{IAPV){l|Ts`i=)AW;d9&KPc3fMeoTS%8@V~D8*h;&(^>yjT84MM}=%#LS7shLAuuj(0VAYoozhWjq z4LEr?wUe2^WGwdTIgWBkDUJa>YP@5d9^Rs$kCXmMRxuF*YMVrn?0NFyPl}>`&dqZb z<5eqR=ZG3>n2{6v6BvJ`YBZeeTtB88TAY(x0a58EWyuf>+^|x8Qa6wA|1Nb_p|nA zWWa}|z8a)--Wj`LqyFk_a3gN2>5{Rl_wbW?#by7&i*^hRknK%jwIH6=dQ8*-_{*x0j^DUfMX0`|K@6C<|1cgZ~D(e5vBFFm;HTZF(!vT8=T$K+|F)x3kqzBV4-=p1V(lzi(s7jdu0>LD#N=$Lk#3HkG!a zIF<7>%B7sRNzJ66KrFV76J<2bdYhxll0y2^_rdG=I%AgW4~)1Nvz=$1UkE^J%BxLo z+lUci`UcU062os*=`-j4IfSQA{w@y|3}Vk?i;&SSdh8n+$iHA#%ERL{;EpXl6u&8@ zzg}?hkEOUOJt?ZL=pWZFJ19mI1@P=$U5*Im1e_8Z${JsM>Ov?nh8Z zP5QvI!{Jy@&BP48%P2{Jr_VgzW;P@7)M9n|lDT|Ep#}7C$&ud&6>C^5ZiwKIg2McPU(4jhM!BD@@L(Gd*Nu$ji(ljZ<{FIeW_1Mmf;76{LU z-ywN~=uNN)Xi6$<12A9y)K%X|(W0p|&>>4OXB?IiYr||WKDOJPxiSe01NSV-h24^L z_>m$;|C+q!Mj**-qQ$L-*++en(g|hw;M!^%_h-iDjFHLo-n3JpB;p?+o2;`*jpvJU zLY^lt)Un4joij^^)O(CKs@7E%*!w>!HA4Q?0}oBJ7Nr8NQ7QmY^4~jvf0-`%waOLn zdNjAPaC0_7c|RVhw)+71NWjRi!y>C+Bl;Z`NiL^zn2*0kmj5gyhCLCxts*cWCdRI| zjsd=sT5BVJc^$GxP~YF$-U{-?kW6r@^vHXB%{CqYzU@1>dzf#3SYedJG-Rm6^RB7s zGM5PR(yKPKR)>?~vpUIeTP7A1sc8-knnJk*9)3t^e%izbdm>Y=W{$wm(cy1RB-19i za#828DMBY+ps#7Y8^6t)=Ea@%Nkt)O6JCx|ybC;Ap}Z@Zw~*}3P>MZLPb4Enxz9Wf zssobT^(R@KuShj8>@!1M7tm|2%-pYYDxz-5`rCbaTCG5{;Uxm z*g=+H1X8{NUvFGzz~wXa%Eo};I;~`37*WrRU&K0dPSB$yk(Z*@K&+mFal^?c zurbqB-+|Kb5|sznT;?Pj!+kgFY1#Dr;_%A(GIQC{3ct|{*Bji%FNa6c-thbpBkA;U zURV!Dr&X{0J}iht#-Qp2=xzuh(fM>zRoiGrYl5ttw2#r34gC41CCOC31m~^UPTK@s z6;A@)7O7_%C)>bnAXerYuAHdE93>j2N}H${zEc6&SbZ|-fiG*-qtGuy-qDelH(|u$ zorf8_T6Zqe#Ub!+e3oSyrskt_HyW_^5lrWt#30l)tHk|j$@YyEkXUOV;6B51L;M@=NIWZXU;GrAa(LGxO%|im%7F<-6N;en0Cr zLH>l*y?pMwt`1*cH~LdBPFY_l;~`N!Clyfr;7w<^X;&(ZiVdF1S5e(+Q%60zgh)s4 zn2yj$+mE=miVERP(g8}G4<85^-5f@qxh2ec?n+$A_`?qN=iyT1?U@t?V6DM~BIlBB z>u~eXm-aE>R0sQy!-I4xtCNi!!qh?R1!kKf6BoH2GG{L4%PAz0{Sh6xpuyI%*~u)s z%rLuFl)uQUCBQAtMyN;%)zFMx4loh7uTfKeB2Xif`lN?2gq6NhWhfz0u5WP9J>=V2 zo{mLtSy&BA!mSzs&CrKWq^y40JF5a&GSXIi2= z{EYb59J4}VwikL4P=>+mc6{($FNE@e=VUwG+KV21;<@lrN`mnz5jYGASyvz7BOG_6(p^eTxD-4O#lROgon;R35=|nj#eHIfJBYPWG>H>`dHKCDZ3`R{-?HO0mE~(5_WYcFmp8sU?wr*UkAQiNDGc6T zA%}GOLXlOWqL?WwfHO8MB#8M8*~Y*gz;1rWWoVSXP&IbKxbQ8+s%4Jnt?kDsq7btI zCDr0PZ)b;B%!lu&CT#RJzm{l{2fq|BcY85`w~3LSK<><@(2EdzFLt9Y_`;WXL6x`0 zDoQ?=?I@Hbr;*VVll1Gmd8*%tiXggMK81a+T(5Gx6;eNb8=uYn z5BG-0g>pP21NPn>$ntBh>`*})Fl|38oC^9Qz>~MAazH%3Q~Qb!ALMf$srexgPZ2@&c~+hxRi1;}+)-06)!#Mq<6GhP z-Q?qmgo${aFBApb5p}$1OJKTClfi8%PpnczyVKkoHw7Ml9e7ikrF0d~UB}i3vizos zXW4DN$SiEV9{faLt5bHy2a>33K%7Td-n5C*N;f&ZqAg#2hIqEb(y<&f4u5BWJ>2^4 z414GosL=Aom#m&=x_v<0-fp1r%oVJ{T-(xnomNJ(Dryv zh?vj+%=II_nV+@NR+(!fZZVM&(W6{6%9cm+o+Z6}KqzLw{(>E86uA1`_K$HqINlb1 zKelh3-jr2I9V?ych`{hta9wQ2c9=MM`2cC{m6^MhlL2{DLv7C^j z$xXBCnDl_;l|bPGMX@*tV)B!c|4oZyftUlP*?$YU9C_eAsuVHJ58?)zpbr30P*C`T z7y#ao`uE-SOG(Pi+`$=e^mle~)pRrdwL5)N;o{gpW21of(QE#U6w%*C~`v-z0QqBML!!5EeYA5IQB0 z^l01c;L6E(iytN!LhL}wfwP7W9PNAkb+)Cst?qg#$n;z41O4&v+8-zPs+XNb-q zIeeBCh#ivnFLUCwfS;p{LC0O7tm+Sf9Jn)~b%uwP{%69;QC)Ok0t%*a5M+=;y8j=v z#!*pp$9@!x;UMIs4~hP#pnfVc!%-D<+wsG@R2+J&%73lK|2G!EQC)O05TCV=&3g)C!lT=czLpZ@Sa%TYuoE?v8T8`V;e$#Zf2_Nj6nvBgh1)2 GZ~q4|mN%#X literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..34bd9ce9 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100644 index 00000000..f5feea6d --- /dev/null +++ b/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 00000000..9d21a218 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,94 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 00000000..969c7b09 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/deploy/logs/.gitkeep b/src/main/deploy/logs/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..23e0db94 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 00000000..a8ea08de --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,36 @@ +{ + "robotWidth": 0.76, + "robotLength": 0.7, + "holonomicMode": true, + "pathFolders": [ + "exampleAuto" + ], + "autoFolders": [], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 4.0, + "defaultMaxAngVel": 400.0, + "defaultMaxAngAccel": 400.0, + "defaultNominalVoltage": 12.0, + "robotMass": 64.0, + "robotMOI": 5.018, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.049, + "driveGearing": 6.12, + "maxDriveSpeed": 4.4, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 100.0, + "wheelCOF": 1.6, + "flModuleX": 0.172, + "flModuleY": 0.273, + "frModuleX": 0.17215, + "frModuleY": -0.273, + "blModuleX": -0.24285, + "blModuleY": 0.273, + "brModuleX": -0.24285, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib new file mode 160000 index 00000000..553e7916 --- /dev/null +++ b/src/main/java/frc/trigon/lib @@ -0,0 +1 @@ +Subproject commit 553e7916e7607dde051530e8903879fb590e573c diff --git a/src/main/java/frc/trigon/robot/Main.java b/src/main/java/frc/trigon/robot/Main.java new file mode 100644 index 00000000..e4b1d052 --- /dev/null +++ b/src/main/java/frc/trigon/robot/Main.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. + +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.trigon.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +public final class Main { + private Main() { + } + + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java new file mode 100644 index 00000000..dcb18ad9 --- /dev/null +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -0,0 +1,95 @@ +// Copyright (c) FIRST and other WPILib contributors. + +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.trigon.robot; + +import edu.wpi.first.wpilibj.Threads; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.hardware.phoenix6.Phoenix6Inputs; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +public class Robot extends LoggedRobot { + public static final boolean IS_REAL = Robot.isReal(); + private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); + private Command autonomousCommand; + private final RobotContainer robotContainer; + + Robot() { + RobotConstants.init(); + configLogger(); + robotContainer = new RobotContainer(); + } + + @Override + public void robotPeriodic() { + Threads.setCurrentThreadPriority(true, 99); + Phoenix6Inputs.refreshAllInputs(); + commandScheduler.run(); + Threads.setCurrentThreadPriority(false, 10); + } + + @Override + public void autonomousInit() { + autonomousCommand = robotContainer.getAutonomousCommand(); + + if (autonomousCommand != null) + autonomousCommand.schedule(); + } + + @Override + public void teleopInit() { + if (autonomousCommand != null) + autonomousCommand.cancel(); + } + + @Override + public void testInit() { + commandScheduler.cancelAll(); + } + + @Override + public void simulationPeriodic() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void testPeriodic() { + } + + private void configLogger() { + if (RobotHardwareStats.isReplay()) { + setUseTiming(false); + final String logPath = LogFileUtil.findReplayLog(); + final String logWriterPath = LogFileUtil.addPathSuffix(logPath, "_replay"); + + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(logWriterPath)); + } else { + Logger.addDataReceiver(new NT4Publisher()); + Logger.addDataReceiver(new WPILOGWriter(RobotConstants.LOGGING_PATH)); + } + + Logger.start(); + } +} diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java new file mode 100644 index 00000000..2e6ec610 --- /dev/null +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. + +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.trigon.robot; + +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.utilities.flippable.Flippable; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.CameraConstants; +import frc.trigon.robot.constants.LEDConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; +import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.swerve.Swerve; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +public class RobotContainer { + public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(); + public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator( + CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, + ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION, + SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE, + CameraConstants.OBJECT_DETECTION_CAMERA + ); + public static final Swerve SWERVE = new Swerve(); + private LoggedDashboardChooser autoChooser; + + public RobotContainer() { + initializeGeneralSystems(); + buildAutoChooser(); + configureBindings(); + } + + /** + * @return the command to run in autonomous mode + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } + + private void configureBindings() { + bindDefaultCommands(); + bindControllerCommands(); + } + + private void bindDefaultCommands() { + SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); + } + + private void bindControllerCommands() { + OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); + OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); + OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + } + + private void configureSysIDBindings(MotorSubsystem subsystem) { + OperatorConstants.FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getQuasistaticCharacterizationCommand(SysIdRoutine.Direction.kForward)); + OperatorConstants.BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getQuasistaticCharacterizationCommand(SysIdRoutine.Direction.kReverse)); + OperatorConstants.FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getDynamicCharacterizationCommand(SysIdRoutine.Direction.kForward)); + OperatorConstants.BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getDynamicCharacterizationCommand(SysIdRoutine.Direction.kReverse)); + subsystem.setDefaultCommand(Commands.idle(subsystem)); + } + + /** + * Initializes the general systems of the robot. + * Some systems need to be initialized at the start of the robot code so that others can use their functions. + * For example, the LEDConstants need to be initialized so that the other systems can use them. + */ + private void initializeGeneralSystems() { + Flippable.init(); + LEDConstants.init(); + AutonomousConstants.init(); + } + + private void buildAutoChooser() { + autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java new file mode 100644 index 00000000..a9fce76d --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -0,0 +1,122 @@ +package frc.trigon.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.lib.commands.CameraPositionCalculationCommand; +import frc.trigon.lib.commands.WheelRadiusCharacterizationCommand; +import frc.trigon.lib.hardware.misc.XboxController; +import frc.trigon.lib.utilities.flippable.FlippableRotation2d; + +/** + * A class that contains commands that only use parameters and don't require logic. + * These are different from {@link GeneralCommands} and command factories because they don't contain any logic, and only run an existing command with parameters. + * For example, the static LED command always changes all the LED strips to the same LED mode with the same settings. + */ +public class CommandConstants { + private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; + private static final double + MINIMUM_TRANSLATION_SHIFT_POWER = 0.30, + MINIMUM_ROTATION_SHIFT_POWER = 0.4; + private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; + + public static final Command + RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), + SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> 0 + ), + FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + CommandConstants::calculateTargetHeadingFromJoystickAngle + ), + SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) + ), + WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand( + AutonomousConstants.ROBOT_CONFIG.moduleLocations, + RobotContainer.SWERVE::getDriveWheelPositionsRadians, + () -> RobotContainer.SWERVE.getHeading().getRadians(), + (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), + RobotContainer.SWERVE + ), + CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand( + RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, + Rotation2d.fromDegrees(0), + (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), + RobotContainer.SWERVE + ); + + /** + * Calculates the target drive power from an axis value by dividing it by the shift mode value. + * + * @param axisValue the stick's value + * @return the drive power + */ + public static double calculateDriveStickAxisValue(double axisValue) { + return axisValue / OperatorConstants.TRANSLATION_STICK_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER); + } + + /** + * Calculates the target rotation power from an axis value by dividing it by the shift mode value. + * + * @param axisValue the stick's value + * @return the rotation power + */ + public static double calculateRotationStickAxisValue(double axisValue) { + return axisValue / OperatorConstants.ROTATION_STICK_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER); + } + + /** + * The shift mode is a mode of the robot that slows down the robot relative to how much the right trigger axis is pressed. + * This method will take the given power, and slow it down relative to how much the right trigger is pressed. + * + * @param minimumPower the minimum amount of power the shift mode can limit (as an absolute number) + * @return the power to apply to the robot + */ + public static double calculateShiftModeValue(double minimumPower) { + final double squaredShiftModeValue = Math.sqrt(DRIVER_CONTROLLER.getRightTriggerAxis()); + final double minimumShiftValueCoefficient = 1 - (1 / minimumPower); + + return 1 - squaredShiftModeValue * minimumShiftValueCoefficient; + } + + /** + * Calculates the target rotation value from the joystick's angle. Used for joystick oriented rotation. + * Joystick oriented rotation is when the robot rotates directly to the joystick's angle. + * + * @return the target heading + */ + private static FlippableRotation2d calculateTargetHeadingFromJoystickAngle() { + final double + xPower = DRIVER_CONTROLLER.getRightX(), + yPower = DRIVER_CONTROLLER.getRightY(); + + final double joystickPower = Math.hypot(xPower, yPower); + if (joystickPower < JOYSTICK_ORIENTED_ROTATION_DEADBAND) + return null; + + return new FlippableRotation2d(Math.atan2(xPower, yPower), true); + } + + private static double getXPowerFromPov(double pov) { + final double povRadians = Units.degreesToRadians(pov); + return Math.cos(povRadians); + } + + private static double getYPowerFromPov(double pov) { + final double povRadians = Units.degreesToRadians(pov); + return Math.sin(-povRadians); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java new file mode 100644 index 00000000..6a6ffb1e --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -0,0 +1,80 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.lib.utilities.flippable.FlippableRotation2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.littletonrobotics.junction.Logger; + +import java.util.function.Supplier; + +public class GamePieceAutoDriveCommand extends ParallelCommandGroup { + private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR; + private final SimulatedGamePieceConstants.GamePieceType targetGamePieceType; + private Translation2d distanceFromTrackedGamePiece; + + public GamePieceAutoDriveCommand(SimulatedGamePieceConstants.GamePieceType targetGamePieceType) { + this.targetGamePieceType = targetGamePieceType; + addCommands( + getTrackGamePieceCommand(), + GeneralCommands.getContinuousConditionalCommand( + getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece), + GeneralCommands.getFieldRelativeDriveCommand(), + () -> OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) + ) + ); + } + + private Command getTrackGamePieceCommand() { + return new RunCommand(() -> { + distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece(); + }); + } + + public static Translation2d calculateDistanceFromTrackedGamePiece() { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + if (trackedObjectPositionOnField == null) + return null; + + final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField); + var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); + Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece); + Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); + return distanceFromTrackedGamePiece; + } + + public static Command getDriveToGamePieceCommand(Supplier distanceFromTrackedGamePiece) { + return new SequentialCommandGroup( + new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX())), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), + GamePieceAutoDriveCommand::calculateTargetAngle + ) + ); + } + + public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) { + return distanceFromTrackedGamePiece != null && + (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open + } + + public static FlippableRotation2d calculateTargetAngle() { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + if (trackedObjectFieldRelativePosition == null) + return null; + + final Translation2d objectDistanceFromRobot = trackedObjectFieldRelativePosition.minus(robotPose.getTranslation()); + final Rotation2d angularDistanceToObject = new Rotation2d(Math.atan2(objectDistanceFromRobot.getY(), objectDistanceFromRobot.getX())); + return new FlippableRotation2d(angularDistanceToObject, false); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java new file mode 100644 index 00000000..d03cf43f --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -0,0 +1,190 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.littletonrobotics.junction.Logger; + +import java.util.function.Supplier; + +/** + * A command class to assist in the intaking of a game piece. + */ +public class IntakeAssistCommand extends ParallelCommandGroup { + private static final ProfiledPIDController + X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + private Translation2d distanceFromTrackedGamePiece; + + /** + * Creates a new intake assist command of the given assist type. + * + * @param assistMode the type of assistance + */ + public IntakeAssistCommand(AssistMode assistMode) { + addCommands( + getTrackGamePieceCommand(), + GeneralCommands.getContinuousConditionalCommand( + GeneralCommands.getFieldRelativeDriveCommand(), + getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece), + () -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null + ) + ); + } + + /** + * Returns a command that assists the intake of a game piece at the given location. + * This command is for intaking a game piece with a specific robot-relative position. + * To create an intake assist command that selects the closest game piece automatically, use {@link IntakeAssistCommand#IntakeAssistCommand(AssistMode)} instead. + * + * @param assistMode the type of assistance + * @param distanceFromTrackedGamePiece the position of the game piece relative to the robot + * @return the command + */ + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedGamePiece) { + return new SequentialCommandGroup( + new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getX(), + () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getY(), + () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get()) + ) + ); + } + + private Command getTrackGamePieceCommand() { + return new RunCommand(() -> { + if (RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null) + distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece(); + }); + } + + private static Translation2d calculateDistanceFromTrackedCGamePiece() { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d trackedObjectPositionOnField = RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + if (trackedObjectPositionOnField == null) + return null; + + final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField); + final Translation2d robotToTrackedGamepieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); + Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamepieceDistance); + return robotToTrackedGamepieceDistance; + } + + private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) { + final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); + final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus()); + + final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getX())); + final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getY())); + + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + } + + private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) { + return calculateThetaAssistPower(assistMode, distanceFromTrackedGamepiece.getAngle().plus(Rotation2d.k180deg).unaryMinus()); + } + + private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + final double pidScalar = Math.cbrt(joystickValue.getNorm()); + final double + xJoystickPower = Math.cbrt(joystickValue.getX()), + yJoystickPower = Math.cbrt(joystickValue.getY()); + final double + xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower), + yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower); + + return new Translation2d(xPower, yPower); + } + + private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + final double + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); + final double + xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; + + return new Translation2d(xPower, yPower); + } + + private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) { + final double + pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())), + joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); + + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); + return calculateNormalAssistPower(pidOutput, joystickValue); + } + + private static double clampToOutputRange(double value) { + return MathUtil.clamp(value, -1, 1); + } + + private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { + return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower; + } + + private static double calculateNormalAssistPower(double pidOutput, double joystickPower) { + final double intakeAssistScalar = OperatorConstants.INTAKE_ASSIST_SCALAR; + return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar)); + } + + private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) { + X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX()); + Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().getY()); + THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond()); + } + + /** + * An enum containing different modes in which the command assists the intake of the game piece. + */ + public enum AssistMode { + /** + * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. + */ + ALTERNATE_ASSIST(true, true, true), + /** + * Applies pid values to autonomously drive to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ + FULL_ASSIST(true, true, true), + /** + * Applies pid values to align to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ + ALIGN_ASSIST(false, true, true), + /** + * Applies pid values to face the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ + ASSIST_ROTATION(false, false, true); + + final boolean + shouldAssistX, + shouldAssistY, + shouldAssistTheta; + + AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { + this.shouldAssistX = shouldAssistX; + this.shouldAssistY = shouldAssistY; + this.shouldAssistTheta = shouldAssistTheta; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java new file mode 100644 index 00000000..aa76f81f --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java @@ -0,0 +1,72 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.AutonomousCommands; +import org.littletonrobotics.junction.Logger; + +import java.util.function.Supplier; + +/** + * A command that sets the LED strips to a color based on the robot's position and orientation relative to the starting + * pose of the selected autonomous path. + * This is very useful for placing the robot in the correct starting position and orientation for autonomous mode before a match. + */ +public class LEDAutoSetupCommand extends SequentialCommandGroup { + private static final double + TOLERANCE_METERS = 0.1, + TOLERANCE_DEGREES = 4; + private final Supplier autoName; + private Pose2d autoStartPose; + + /** + * Constructs a new LEDAutoSetupCommand. + * + * @param autoName a supplier that returns the name of the selected autonomous path + */ + public LEDAutoSetupCommand(Supplier autoName) { + this.autoName = autoName; + + addCommands( + getUpdateAutoStartPoseCommand() + ); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + + private Command getUpdateAutoStartPoseCommand() { + return new InstantCommand(() -> { + this.autoStartPose = AutonomousCommands.getAutoStartPose(autoName.get()); + Logger.recordOutput("PathPlanner/AutoStartPose", autoStartPose); + }); + } + + private Translation2d calculateRobotRelativeDifference() { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d robotRelativeRobotTranslation = robotPose.getTranslation().minus(this.autoStartPose.getTranslation()); + return robotRelativeRobotTranslation.rotateBy(robotPose.getRotation()); + } + + /** + * Gets the correct color of a section of the LED based on the difference of between the auto start pose and the current robot pose. + * + * @param differenceMeters the difference between the robot's position and the auto's start position + * @param toleranceMeters the maximum distance from the auto start position to display as the correct start position + * @return the desired color + */ + private Color getDesiredLEDColorFromRobotPose(double differenceMeters, double toleranceMeters) { + if (differenceMeters < -toleranceMeters) + return Color.kBlack; + else if (differenceMeters > toleranceMeters) + return Color.kRed; + return Color.kGreen; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java new file mode 100644 index 00000000..4cfe5aa2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -0,0 +1,149 @@ +package frc.trigon.robot.commands.commandfactories; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.GamePieceAutoDriveCommand; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.json.simple.parser.ParseException; + +import java.io.IOException; +import java.util.function.Supplier; + +/** + * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match. + */ +public class AutonomousCommands { + private static FlippablePose2d TARGET_SCORING_POSE = null; + + /** + * Creates a dynamic autonomous command intended for the 15-second autonomous period at the beginning of a match. + * Dynamic means that the command isn't pre-programmed and instead autonomously decides what game pieces to collect and where to score. + * + * @param intakeLocations the locations at which to collect game pieces + * @param scoringLocations the locations at which to score + * @return the command + */ + public static Command getDynamicAutonmousCommand(FlippablePose2d[] intakeLocations, FlippablePose2d... scoringLocations) { + return new SequentialCommandGroup( + getDriveAndScoreCommand(scoringLocations), + getCollectCommand(intakeLocations) + ).repeatedly().withName(generateDynamicAutonomousRoutineName(intakeLocations, scoringLocations)); + } + + private static Command getCollectCommand(FlippablePose2d[] intakeLocations) { + return new ParallelCommandGroup( + getIntakeSequenceCommand(), + getDriveToGamePieceCommand(intakeLocations) + ).until(AutonomousCommands::hasGamePiece); + } + + private static boolean hasGamePiece() { + //TODO: implement + return false; + } + + private static Command getDriveToGamePieceCommand(FlippablePose2d[] intakeLocations) { + return new ConditionalCommand( + new GamePieceAutoDriveCommand(SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE).onlyWhile(() -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + getFindGamePieceCommand(intakeLocations), + AutonomousCommands::shouldCollectGamePiece + ); + } + + private static boolean shouldCollectGamePiece() { + return RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null; + } + + private static Command getFindGamePieceCommand(FlippablePose2d[] intakeLocations) { + //TODO: implement + return null; + } + + private static Command getIntakeSequenceCommand() { + //TODO: implement + return null; + } + + private static Command getDriveAndScoreCommand(FlippablePose2d[] scoringLocations) { + return new ParallelCommandGroup( + getDriveToScoringLocationCommand(scoringLocations), + getScoringSequenceCommand() + ); + } + + private static Command getScoringSequenceCommand() { + return new SequentialCommandGroup( + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), + getScoreCommand() + ); + } + + private static Command getScoreCommand() { + //TODO: implement + return null; + } + + private static Command getDriveToScoringLocationCommand(FlippablePose2d[] scoringLocations) { + return new SequentialCommandGroup( + new InstantCommand(() -> TARGET_SCORING_POSE = calculateBestOpenScoringPose(scoringLocations)), + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_SCORING_LOCATION_CONSTRAINTS).repeatedly() + ); + } + + private static FlippablePose2d calculateBestOpenScoringPose(FlippablePose2d[] scoringLocations) { + //TODO: implement + return null; + } + + /** + * Generates a name for the dynamic autonomous routine based on the intake and scoring locations. + * + * @param intakeLocations the intake locations + * @param scoringLocations the scoring locations + * @return the name of the dynamic autonomous routine + */ + private static String generateDynamicAutonomousRoutineName(FlippablePose2d[] intakeLocations, FlippablePose2d[] scoringLocations) { + //TODO: implement + return ""; + } + + /** + * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled. + * + * @param autoName the name of the autonomous + * @return a command that resets the robot's pose estimator pose to the start position of the given autonomous + */ + public static Command getResetPoseToAutoPoseCommand(Supplier autoName) { + return new InstantCommand( + () -> { + if (DriverStation.isEnabled()) + return; + RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get())); + } + ).ignoringDisable(true); + } + + /** + * Gets the starting position of the target PathPlanner autonomous. + * + * @param autoName the name of the autonomous group + * @return the staring pose of the autonomous + */ + public static Pose2d getAutoStartPose(String autoName) { + try { + final Pose2d nonFlippedAutoStartPose = PathPlannerAuto.getPathGroupFromAutoFile(autoName).get(0).getStartingHolonomicPose().get(); + final FlippablePose2d flippedAutoStartPose = new FlippablePose2d(nonFlippedAutoStartPose, true); + return flippedAutoStartPose.get(); + } catch (IOException | ParseException e) { + e.printStackTrace(); + return new Pose2d(); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java new file mode 100644 index 00000000..5fbedf61 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -0,0 +1,65 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; + +import java.util.function.BooleanSupplier; + +/** + * A class that contains the general commands of the robot, such as commands that alter a command or commands that affect all subsystems. + * These are different from {@link CommandConstants} because they create new commands that use some form of logic instead of only constructing an existing command with parameters. + */ +public class GeneralCommands { + public static Command getFieldRelativeDriveCommand() { + return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()) + ); + } + + public static Command getToggleBrakeCommand() { + return new InstantCommand(() -> { + MotorSubsystem.IS_BRAKING = !MotorSubsystem.IS_BRAKING; + MotorSubsystem.setAllSubsystemsBrakeAsync(MotorSubsystem.IS_BRAKING); + }).ignoringDisable(true); + } + + public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { + return new WaitCommand(delaySeconds).andThen(toRun).ignoringDisable(true); + } + + public static Command getContinuousConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { + return new ConditionalCommand( + onTrue.onlyWhile(condition), + onFalse.until(condition), + condition + ).repeatedly(); + } + + /** + * A command that only runs when a condition is met. + * + * @param command the command to run + * @param condition the condition that needs to be met for the command to run + * @return the command + */ + public static Command runWhen(Command command, BooleanSupplier condition) { + return new WaitUntilCommand(condition).andThen(command); + } + + /** + * A command that only runs when a condition is met for a certain amount of time. + * + * @param command the command to run + * @param condition the condition that needs to be met for the command to run + * @param debounceTimeSeconds the time that the condition needs to be true for the command to run + * @return the command + */ + public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { + return runWhen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition)), condition); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java new file mode 100644 index 00000000..3cce9bae --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -0,0 +1,87 @@ +package frc.trigon.robot.constants; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.utilities.LocalADStarAK; +import frc.trigon.lib.utilities.flippable.Flippable; +import frc.trigon.robot.RobotContainer; +import org.json.simple.parser.ParseException; + +import java.io.IOException; + +/** + * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. + */ +public class AutonomousConstants { + public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; + public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); + public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate + public static final PathConstraints DRIVE_TO_SCORING_LOCATION_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)); + + private static final PIDConstants + AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(0, 0, 0) : + new PIDConstants(0, 0, 0), + AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(0, 0, 0) : + new PIDConstants(0, 0, 0); + + + public static final PIDController GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new PIDController(0.5, 0, 0) : + new PIDController(0.3, 0, 0.03); + public static final ProfiledPIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 2; + + private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( + AUTO_TRANSLATION_PID_CONSTANTS, + AUTO_ROTATION_PID_CONSTANTS + ); + + /** + * Initializes PathPlanner. This needs to be called before any PathPlanner function can be used. + */ + public static void init() { + Pathfinding.setPathfinder(new LocalADStarAK()); + PathfindingCommand.warmupCommand().schedule(); + configureAutoBuilder(); + registerCommands(); + } + + private static void configureAutoBuilder() { + AutoBuilder.configure( + RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, + RobotContainer.ROBOT_POSE_ESTIMATOR::resetPose, + RobotContainer.SWERVE::getSelfRelativeChassisSpeeds, + (chassisSpeeds -> RobotContainer.SWERVE.drivePathPlanner(chassisSpeeds, true)), + AUTO_PATH_FOLLOWING_CONTROLLER, + ROBOT_CONFIG, + Flippable::isRedAlliance, + RobotContainer.SWERVE + ); + } + + private static RobotConfig getRobotConfig() { + try { + return RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + throw new RuntimeException(e); + } + } + + private static void registerCommands() { + //TODO: Implement + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java new file mode 100644 index 00000000..1b750328 --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.constants; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; + +public class CameraConstants { + private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, Units.degreesToRadians(0), 0) + ); + + public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0.5; + public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( + "ObjectDetectionCamera", + ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA + ); +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java new file mode 100644 index 00000000..a2af1a5e --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -0,0 +1,47 @@ +package frc.trigon.robot.constants; + +import com.pathplanner.lib.util.FlippingUtil; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.lib.utilities.FilesHandler; + +import java.io.IOException; +import java.util.HashMap; +import java.util.List; + +public class FieldConstants { + public static final double + FIELD_WIDTH_METERS = FlippingUtil.fieldSizeY, + FIELD_LENGTH_METERS = FlippingUtil.fieldSizeX; + private static final List I_HATE_YOU = List.of( + //Tags to ignore + ); + + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); + private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); + + private static AprilTagFieldLayout createAprilTagFieldLayout() { + try { + return SHOULD_USE_HOME_TAG_LAYOUT ? + new AprilTagFieldLayout(FilesHandler.DEPLOY_PATH + "field_calibration.json") : + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + private static HashMap fieldLayoutToTagIDToPoseMap() { + final HashMap tagIDToPose = new HashMap<>(); + for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) + if (!I_HATE_YOU.contains(aprilTag.ID)) + tagIDToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); + + return tagIDToPose; + } +} diff --git a/src/main/java/frc/trigon/robot/constants/LEDConstants.java b/src/main/java/frc/trigon/robot/constants/LEDConstants.java new file mode 100644 index 00000000..3fea33ed --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/LEDConstants.java @@ -0,0 +1,11 @@ +package frc.trigon.robot.constants; + +public class LEDConstants { + //TODO: Implement LEDConstants + + /** + * Initializes LEDConstants. Needed to be called for the LED strips to be initialized before being used. + */ + public static void init() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java new file mode 100644 index 00000000..93323cc0 --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -0,0 +1,37 @@ +package frc.trigon.robot.constants; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.lib.hardware.misc.KeyboardController; +import frc.trigon.lib.hardware.misc.XboxController; + +public class OperatorConstants { + public static final double DRIVER_CONTROLLER_DEADBAND = 0.07; + private static final int DRIVER_CONTROLLER_PORT = 0; + private static final int + DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1, + DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2; + public static final XboxController DRIVER_CONTROLLER = new XboxController( + DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND + ); + public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); + + public static final double + POV_DIVIDER = 2, + TRANSLATION_STICK_SPEED_DIVIDER = 1, + ROTATION_STICK_SPEED_DIVIDER = 1; + + public static final double INTAKE_ASSIST_SCALAR = 0.0; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + + public static final Trigger + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), + TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), + DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), + FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), + BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), + FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java new file mode 100644 index 00000000..cd08b536 --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -0,0 +1,17 @@ +package frc.trigon.robot.constants; + +import frc.trigon.robot.Robot; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.utilities.FilesHandler; + +public class RobotConstants { + public static final String CANIVORE_NAME = "SwerveCANivore"; + public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/"; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE; + private static final double PERIODIC_TIME_SECONDS = 0.02; + + public static void init() { + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, REPLAY_TYPE); + RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); + } +} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java new file mode 100644 index 00000000..b09d2ec4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -0,0 +1,108 @@ +package frc.trigon.robot.misc.objectdetectioncamera; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import org.littletonrobotics.junction.Logger; +import frc.trigon.lib.hardware.RobotHardwareStats; + +/** + * An object detection camera is a class that represents a camera that detects objects other than apriltags, most likely game pieces. + */ +public class ObjectDetectionCamera extends SubsystemBase { + private final ObjectDetectionCameraInputsAutoLogged objectDetectionCameraInputs = new ObjectDetectionCameraInputsAutoLogged(); + private final ObjectDetectionCameraIO objectDetectionCameraIO; + private final String hostname; + private final Transform3d robotCenterToCamera; + + public ObjectDetectionCamera(String hostname, Transform3d robotCenterToCamera) { + this.hostname = hostname; + this.robotCenterToCamera = robotCenterToCamera; + this.objectDetectionCameraIO = generateIO(hostname, robotCenterToCamera); + } + + @Override + public void periodic() { + objectDetectionCameraIO.updateInputs(objectDetectionCameraInputs); + Logger.processInputs(hostname, objectDetectionCameraInputs); + } + + /** + * Calculates the position of the best object on the field from the 3D rotation of the object relative to the camera. + * This assumes the object is on the ground. + * Once it is known that the object is on the ground, + * one can simply find the transform from the camera to the ground and apply it to the object's rotation. + * + * @return the best object's 2D position on the field (z is assumed to be 0) + */ + public Translation2d calculateBestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece); + final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + if (targetObjectsTranslation.length == 0) + return null; + Translation2d bestObjectTranslation = targetObjectsTranslation[0]; + + for (int i = 1; i < targetObjectsTranslation.length; i++) { + final Translation2d currentObjectTranslation = targetObjectsTranslation[i]; + final double bestObjectDifference = currentRobotTranslation.getDistance(bestObjectTranslation); + final double currentObjectDifference = currentRobotTranslation.getDistance(currentObjectTranslation); + if (currentObjectDifference < bestObjectDifference) + bestObjectTranslation = currentObjectTranslation; + } + return bestObjectTranslation; + } + + public boolean hasTargets(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + return objectDetectionCameraInputs.hasTarget[targetGamePiece.id]; + } + + public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + final Rotation3d[] visibleObjectsRotations = getTargetObjectsRotations(targetGamePiece); + final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length]; + + for (int i = 0; i < visibleObjectsRotations.length; i++) + objectsPositionsOnField[i] = calculateObjectPositionFromRotation(visibleObjectsRotations[i]); + + Logger.recordOutput("ObjectDetectionCamera/Visible" + targetGamePiece.name(), objectsPositionsOnField); + return objectsPositionsOnField; + } + + public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]; + } + + /** + * Calculates the position of the object on the field from the 3D rotation of the object relative to the camera. + * This assumes the object is on the ground. + * Once it is known that the object is on the ground, + * one can simply find the transform from the camera to the ground and apply it to the object's rotation. + * + * @param objectRotation the object's 3D rotation relative to the camera + * @return the object's 2D position on the field (z is assumed to be 0) + */ + private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotation) { + final Pose2d robotPoseAtResultTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(objectDetectionCameraInputs.latestResultTimestamp); + if (robotPoseAtResultTimestamp == null) + return new Translation2d(); + final Pose3d cameraPose = new Pose3d(robotPoseAtResultTimestamp).plus(robotCenterToCamera); + final Pose3d objectRotationStart = cameraPose.plus(new Transform3d(0, 0, 0, objectRotation)); + + final double cameraZ = cameraPose.getTranslation().getZ(); + final double objectPitchSin = Math.sin(objectRotationStart.getRotation().getY()); + final double xTransform = cameraZ / objectPitchSin; + final Transform3d objectRotationStartToGround = new Transform3d(xTransform, 0, 0, new Rotation3d()); + + return objectRotationStart.transformBy(objectRotationStartToGround).getTranslation().toTranslation2d(); + } + + private ObjectDetectionCameraIO generateIO(String hostname, Transform3d robotCenterToCamera) { + if (RobotHardwareStats.isReplay()) + return new ObjectDetectionCameraIO(); + if (RobotHardwareStats.isSimulation()) + return new SimulationObjectDetectionCameraIO(hostname, robotCenterToCamera); + return new PhotonObjectDetectionCameraIO(hostname); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java new file mode 100644 index 00000000..046781ad --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -0,0 +1,10 @@ +package frc.trigon.robot.misc.objectdetectioncamera; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; + +public class ObjectDetectionCameraConstants { + public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; + static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; + public static final Rotation2d LOLLIPOP_TOLERANCE = Rotation2d.fromDegrees(3.5); +} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java new file mode 100644 index 00000000..4d0f336b --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.misc.objectdetectioncamera; + +import edu.wpi.first.math.geometry.Rotation3d; +import org.littletonrobotics.junction.AutoLog; + +public class ObjectDetectionCameraIO { + protected ObjectDetectionCameraIO() { + } + + protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { + } + + @AutoLog + public static class ObjectDetectionCameraInputs { + /** + * Whether there is at least one target or not for each game piece, by game piece index (type). + */ + public boolean[] hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + /** + * Stores the Rotation3d of all visible objects. + * The first index is the game piece ID (type). + * The second index is the index of the game piece's Rotation3d, with the best object placed first (index 0). + */ + public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + public double latestResultTimestamp = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java new file mode 100644 index 00000000..64ea2cfe --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -0,0 +1,200 @@ +package frc.trigon.robot.misc.objectdetectioncamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; +import java.util.HashMap; + +public class ObjectPoseEstimator extends SubsystemBase { + private final HashMap knownObjectPositions; + private final ObjectDetectionCamera camera; + private final double deletionThresholdSeconds; + private final SimulatedGamePieceConstants.GamePieceType gamePieceType; + private final double rotationToTranslation; + + /** + * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. + * + * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed + * @param gamePieceType the type of game piece to track + * @param camera the camera used for detecting objects + */ + public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, + SimulatedGamePieceConstants.GamePieceType gamePieceType, + ObjectDetectionCamera camera) { + this.deletionThresholdSeconds = deletionThresholdSeconds; + this.gamePieceType = gamePieceType; + this.camera = camera; + this.knownObjectPositions = new HashMap<>(); + this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; + } + + /** + * Updates the object positions based on the camera detected objects. + * Removes objects that have not been detected for {@link ObjectPoseEstimator#deletionThresholdSeconds}. + */ + @Override + public void periodic() { + updateObjectPositions(); + removeOldObjects(); + Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); + } + + /** + * Gets the position of all known objects on the field. + * + * @return a list of Translation2d representing the positions of objects on the field + */ + public ArrayList getObjectsOnField() { + return new ArrayList<>(knownObjectPositions.keySet()); + } + + /** + * Removes the closest object to the robot from the list of objects in the pose estimator. + */ + public void removeClosestObjectToRobot() { + final Translation2d closestObject = getClosestObjectToRobot(); + removeObject(closestObject); + } + + /** + * Removes the closest object to the intake from the list of objects in the pose estimator. + * + * @param intakeTransform the transform of the intake relative to the robot + */ + public void removeClosestObjectToIntake(Transform2d intakeTransform) { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + removeObject(getClosestObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); + } + + /** + * Removes the closest object to a given pose from the list of objects in the pose estimator. + * + * @param fieldRelativePose the pose to which the removed object is closest + */ + public void removeClosestObjectToPose(Pose2d fieldRelativePose) { + final Translation2d closestObject = getClosestObjectToPosition(fieldRelativePose.getTranslation()); + removeObject(closestObject); + } + + public void removeClosestObjectToPosition(Translation2d position) { + final Translation2d closestObject = getClosestObjectToPosition(position); + removeObject(closestObject); + } + + /** + * Removes a specific object from the list of known objects in the pose estimator. + * + * @param objectPosition the position of the object to be removed. Must be the precise position as stored in the pose estimator. + */ + public void removeObject(Translation2d objectPosition) { + knownObjectPositions.remove(objectPosition); + } + + /** + * Gets the position of the closest object on the field from the 3D rotation of the object relative to the camera. + * This assumes the object is on the ground. + * Once it is known that the object is on the ground, + * one can simply find the transform from the camera to the ground and apply it to the object's rotation. + * + * @return the best object's 2D position on the field (z is assumed to be 0) + */ + public Translation2d getClosestObjectToRobot() { + return getClosestObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + } + + + /** + * Gets the position of the closest object to a given position on the field. + * + * @param position the position to which the returned object is closest + * @return the closest object's position on the field, or null if no objects are known + */ + public Translation2d getClosestObjectToPosition(Translation2d position) { + final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); + if (knownObjectPositions.isEmpty()) + return null; + Translation2d bestObjectTranslation = objectsTranslations[0]; + double closestObjectDistance = position.getDistance(bestObjectTranslation); + + for (int i = 1; i < objectsTranslations.length; i++) { + final Translation2d currentObjectTranslation = objectsTranslations[i]; + final double currentObjectDistance = position.getDistance(currentObjectTranslation); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; + bestObjectTranslation = currentObjectTranslation; + } + } + return bestObjectTranslation; + } + + /** + * Calculates the "distance rating" of an object. + * The "distance rating" is a unit used to calculate the distance between 2 poses. + * It factors in both translation and rotation differences by scaling the units depending on the {@link DistanceCalculationMethod}. + * + * @param objectTranslation the translation of the object on the field + * @param pose the pose to which the distance is measured from + * @return the objects "distance rating" + */ + private double calculateObjectDistanceRating(Translation2d objectTranslation, Pose2d pose) { + final double translationDifference = pose.getTranslation().getDistance(objectTranslation); + final double xDifference = Math.abs(pose.getX() - objectTranslation.getX()); + final double yDifference = Math.abs(pose.getY() - objectTranslation.getY()); + final double rotationDifferenceDegrees = Math.abs(pose.getRotation().getDegrees() - Math.atan2(yDifference, xDifference)); + return translationDifference * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); + } + + private void updateObjectPositions() { + final double currentTimestamp = Timer.getTimestamp(); + for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { + Translation2d closestObjectToVisibleObject = new Translation2d(); + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d knownObject : knownObjectPositions.keySet()) { + final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = knownObject; + } + } + + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) + removeObject(closestObjectToVisibleObject); + knownObjectPositions.put(visibleObject, currentTimestamp); + } + } + + private void removeOldObjects() { + knownObjectPositions.entrySet().removeIf(entry -> isTooOld(entry.getValue())); + } + + private boolean isTooOld(double timestamp) { + return Timer.getTimestamp() - timestamp > deletionThresholdSeconds; + } + + public enum DistanceCalculationMethod { + ROTATION(0), + TRANSLATION(1), + ROTATION_AND_TRANSLATION(0.1); + + /** + * The ratio of rotation to translation in the distance rating calculation. + * A value of 0 means only rotation is considered, 1 means only translation is considered. + * Values in between are the ratio of rotation to translation in the distance rating calculation. + * For example, a value of 0.1 means that 9 cm of translation is considered equivalent to 1 degree of rotation. + */ + final double rotationToTranslation; + + DistanceCalculationMethod(double rotationToTranslation) { + this.rotationToTranslation = rotationToTranslation; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java new file mode 100644 index 00000000..541f10e8 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -0,0 +1,85 @@ +package frc.trigon.robot.misc.objectdetectioncamera.io; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class PhotonObjectDetectionCameraIO extends ObjectDetectionCameraIO { + private final PhotonCamera photonCamera; + + public PhotonObjectDetectionCameraIO(String hostname) { + PhotonCamera.setVersionCheckEnabled(false); + photonCamera = new PhotonCamera(hostname); + } + + @Override + protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { + if (!photonCamera.isConnected()) { + updateNoNewResultInputs(inputs); + return; + } + + final PhotonPipelineResult result = getLatestPipelineResult(); + if (result == null || !result.hasTargets()) { + updateNoNewResultInputs(inputs); + return; + } + + updateHasNewResultInputs(inputs, result); + } + + private PhotonPipelineResult getLatestPipelineResult() { + final List unreadResults = photonCamera.getAllUnreadResults(); + return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); + } + + private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { + inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + } + + private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, PhotonPipelineResult result) { + final List[] visibleObjectsRotations = new List[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + visibleObjectsRotations[i] = new ArrayList<>(); + Arrays.fill(inputs.hasTarget, false); + inputs.latestResultTimestamp = result.getTimestampSeconds(); + + for (PhotonTrackedTarget currentTarget : result.getTargets()) { + if (currentTarget.getDetectedObjectClassID() == -1) + continue; + + inputs.hasTarget[currentTarget.getDetectedObjectClassID()] = true; + visibleObjectsRotations[currentTarget.getDetectedObjectClassID()].add(extractRotation3d(currentTarget)); + } + + for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + inputs.visibleObjectRotations[i] = toArray(visibleObjectsRotations[i]); + } + + private Rotation3d[] toArray(List list) { + final Rotation3d[] array = new Rotation3d[list.size()]; + + for (int i = 0; i < array.length; i++) + array[i] = list.get(i); + + return array; + } + + private Rotation3d extractRotation3d(PhotonTrackedTarget target) { + return new Rotation3d( + 0, + Units.degreesToRadians(-target.getPitch()), + Units.degreesToRadians(-target.getYaw()) + ); + } +} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java new file mode 100644 index 00000000..a7bd8346 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -0,0 +1,159 @@ +package frc.trigon.robot.misc.objectdetectioncamera.io; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.Timer; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; + +public class SimulationObjectDetectionCameraIO extends ObjectDetectionCameraIO { + private static final Rotation2d + CAMERA_HORIZONTAL_FOV = Rotation2d.fromDegrees(75), + CAMERA_VERTICAL_FOV = Rotation2d.fromDegrees(45); + + private final String hostname; + private final Transform3d robotCenterToCamera; + + public SimulationObjectDetectionCameraIO(String hostname, Transform3d robotCenterToCamera) { + this.hostname = hostname; + this.robotCenterToCamera = robotCenterToCamera; + } + + @Override + protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose3d cameraPose = new Pose3d(robotPose).plus(robotCenterToCamera); + final ArrayList>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose); + + boolean hasAnyTarget = false; + for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { + inputs.hasTarget[i] = !visibleGamePieces[i].isEmpty(); + if (inputs.hasTarget[i]) + hasAnyTarget = true; + } + + if (hasAnyTarget) { + updateHasNewResultInputs(inputs, visibleGamePieces); + return; + } + + updateNoNewResultInputs(inputs); + } + + private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { + final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + for (int i = 0; i < visibleGamePieces.length; i++) + visibleGamePieces[i] = calculateVisibleGamePieces(cameraPose, i); + return visibleGamePieces; + } + + private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { + inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + } + + private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, ArrayList>[] visibleGamePieces) { + for (int i = 0; i < visibleGamePieces.length; i++) { + inputs.visibleObjectRotations[i] = new Rotation3d[visibleGamePieces[i].size()]; + for (int j = 0; j < visibleGamePieces[i].size(); j++) + inputs.visibleObjectRotations[i][j] = visibleGamePieces[i].get(j).getSecond(); + } + + inputs.latestResultTimestamp = Timer.getTimestamp(); + + logVisibleGamePieces(visibleGamePieces); + } + + /** + * Calculates the placements of all visible objects by checking if they are within range and within the horizontal FOV. + * + * @param cameraPose the position of the robot on the field + * @param objectID the ID of the object to check for visibility + * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera + */ + private ArrayList> calculateVisibleGamePieces(Pose3d cameraPose, int objectID) { + final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); + final ArrayList> visibleTargetObjects = new ArrayList<>(); + for (SimulatedGamePiece currentObject : gamePiecesOnField) { + if (currentObject.isScored()) + continue; + final Rotation3d cameraAngleToObject = calculateCameraAngleToObject(currentObject.getPose(), cameraPose); + + if (isWithinFOV(cameraAngleToObject)) + visibleTargetObjects.add(new Pair<>(currentObject, cameraAngleToObject)); + } + + return visibleTargetObjects; + } + + private Rotation3d calculateCameraAngleToObject(Pose3d objectPose, Pose3d cameraPose) { + final Translation3d cameraPosition = cameraPose.getTranslation(); + Translation3d objectPosition = objectPose.getTranslation(); + if (objectPose.getRotation().getZ() < 0.2) + objectPosition = objectPosition.minus(new Translation3d(0, 0, objectPosition.getZ())); + + final Translation3d difference = cameraPosition.minus(objectPosition); + final Rotation3d differenceAsAngle = getAngle(difference); + + + return differenceAsAngle.minus(cameraPose.getRotation()); + } + + private Rotation3d getAngle(Translation3d translation) { + return new Rotation3d(0, getPitch(translation).getRadians(), getYaw(translation).getRadians()); + } + + /** + * Extracts the yaw off of a 3d vector. + * + * @param vector the vector to extract the yaw from + * @return the yaw of the vector + */ + private Rotation2d getYaw(Translation3d vector) { + return new Rotation2d(Math.atan2(-vector.getY(), -vector.getX())); + } + + /** + * Extracts the pitch off of a 3d vector. + * + * @param vector the vector to extract the pitch from + * @return the pitch of the vector + */ + private Rotation2d getPitch(Translation3d vector) { + return new Rotation2d(Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); + } + + /** + * Checks if an object is within the field-of-view of the camera. + * + * @param objectRotation the rotation of the object relative to the camera + * @return if the object is within the field-of-view of the camera + */ + private boolean isWithinFOV(Rotation3d objectRotation) { + return Math.abs(objectRotation.getZ()) <= CAMERA_HORIZONTAL_FOV.getRadians() / 2 && + Math.abs(objectRotation.getY()) <= CAMERA_VERTICAL_FOV.getRadians() / 2; + } + + private void logVisibleGamePieces(ArrayList>[] visibleGamePieces) { + for (int i = 0; i < visibleGamePieces.length; i++) { + final String gamePieceTypeName = SimulatedGamePieceConstants.GamePieceType.getNameFromID(i); + Logger.recordOutput(hostname + "/Visible" + gamePieceTypeName + "poses", mapSimulatedGamePieceListToPoseArray(visibleGamePieces[i])); + } + } + + private Pose3d[] mapSimulatedGamePieceListToPoseArray(ArrayList> gamePieces) { + final Pose3d[] poses = new Pose3d[gamePieces.size()]; + for (int i = 0; i < poses.length; i++) + poses[i] = gamePieces.get(i).getFirst().getPose(); + + return poses; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java new file mode 100644 index 00000000..a27b8752 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java @@ -0,0 +1,102 @@ +package frc.trigon.robot.misc.simulatedfield; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; + +public class SimulatedGamePiece { + protected SimulatedGamePieceConstants.GamePieceType gamePieceType; + protected boolean isScored = false; + private Pose3d fieldRelativePose; + private Pose3d poseAtRelease; + private Translation3d velocityAtRelease = new Translation3d(); + private double timestampAtRelease = 0; + private boolean isTouchingGround = true; + + public SimulatedGamePiece(Pose3d startingPose, SimulatedGamePieceConstants.GamePieceType gamePieceType) { + fieldRelativePose = startingPose; + this.gamePieceType = gamePieceType; + } + + public void updatePeriodically(boolean isHeld) { + if (!isHeld) + checkScored(); + if (!isScored && !isTouchingGround) + applyGravity(); + } + + /** + * Releases the game piece from the robot. + * + * @param fieldRelativeReleaseVelocity the velocity that the object is released at, relative to the field + */ + public void release(Translation3d fieldRelativeReleaseVelocity) { + velocityAtRelease = fieldRelativeReleaseVelocity; + poseAtRelease = fieldRelativePose; + timestampAtRelease = Timer.getTimestamp(); + + updateIsTouchingGround(); + } + + public void updatePose(Pose3d fieldRelativePose) { + this.fieldRelativePose = fieldRelativePose; + } + + public Pose3d getPose() { + return fieldRelativePose; + } + + public double getDistanceFromPoseMeters(Pose3d pose) { + return fieldRelativePose.minus(pose).getTranslation().getNorm(); + } + + public boolean isScored() { + return isScored; + } + + private void applyGravity() { + if (poseAtRelease == null) + return; + final double timeSinceEject = Timer.getTimestamp() - timestampAtRelease; + this.fieldRelativePose = new Pose3d(poseAtRelease.getTranslation(), new Rotation3d()).transformBy(calculateVelocityPoseTransform(timeSinceEject)); + + updateIsTouchingGround(); + } + + private Transform3d calculateVelocityPoseTransform(double elapsedTime) { + return new Transform3d( + velocityAtRelease.getX() * elapsedTime, + velocityAtRelease.getY() * elapsedTime, + velocityAtRelease.getZ() * elapsedTime - ((SimulatedGamePieceConstants.G_FORCE / 2) * elapsedTime * elapsedTime), + poseAtRelease.getRotation() + ); + } + + private void checkScored() { + if (!isScored) + SimulationScoringHandler.checkGamePieceScored(this); + } + + private void updateIsTouchingGround() { + if (fieldRelativePose.getZ() < gamePieceType.originPointHeightOffGroundMeters) { + isTouchingGround = true; + velocityAtRelease = new Translation3d(); + + final Translation3d fieldRelativeTranslation = new Translation3d( + fieldRelativePose.getTranslation().getX(), + fieldRelativePose.getTranslation().getY(), + gamePieceType.originPointHeightOffGroundMeters + ); + final Rotation3d fieldRelativeRotation = new Rotation3d( + fieldRelativePose.getRotation().getX(), + 0, + fieldRelativePose.getRotation().getZ() + ); + fieldRelativePose = new Pose3d(fieldRelativeTranslation, fieldRelativeRotation); + return; + } + isTouchingGround = false; + } +} diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java new file mode 100644 index 00000000..b7dd1e23 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -0,0 +1,47 @@ +package frc.trigon.robot.misc.simulatedfield; + +import edu.wpi.first.math.geometry.Rotation3d; +import frc.trigon.lib.utilities.flippable.FlippablePose3d; +import frc.trigon.lib.utilities.flippable.FlippableTranslation2d; + +import java.util.ArrayList; +import java.util.List; + +public class SimulatedGamePieceConstants { + public static final double G_FORCE = 9.806; + + public static final double + FEEDER_INTAKE_TOLERANCE_METERS = 0.1, + INTAKE_TOLERANCE_METERS = 0.1, + SCORING_TOLERANCE_METERS = 0.1; + + /** + * Stores all the game pieces. + * Starts out with the game pieces the start on the field. + */ + public static final ArrayList + STARTING_GAME_PIECES = new ArrayList<>(List.of( + )); + + public static final FlippablePose3d SCORING_LOCATION = new FlippablePose3d(0, 0, 0, new Rotation3d(), true); + public static final FlippableTranslation2d FEEDER_POSITION = new FlippableTranslation2d(0, 0, true); + + public enum GamePieceType { + GAME_PIECE_TYPE(0, 0); + + public final double originPointHeightOffGroundMeters; + public final int id; + + GamePieceType(double originPointHeightOffGroundMeters, int id) { + this.originPointHeightOffGroundMeters = originPointHeightOffGroundMeters; + this.id = id; + } + + public static String getNameFromID(int id) { + for (int i = 0; i < values().length; i++) + if (values()[i].id == id) + return values()[i].toString(); + return ""; + } + } +} diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java new file mode 100644 index 00000000..28382454 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -0,0 +1,159 @@ +package frc.trigon.robot.misc.simulatedfield; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.trigon.robot.RobotContainer; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; + +/** + * Handles the simulation of game pieces. + */ +public class SimulationFieldHandler { + private static final ArrayList GAME_PIECES_ON_FIELD = SimulatedGamePieceConstants.STARTING_GAME_PIECES; + private static Integer HELD_GAME_PIECE_INDEX = null; + + public static ArrayList getSimulatedGamePieces() { + return GAME_PIECES_ON_FIELD; + } + + public static boolean isHoldingGamePiece() { + return HELD_GAME_PIECE_INDEX != null; + } + + public static void update() { + updateGamePieces(); + logGamePieces(); + } + + /** + * Updates the state of all game pieces. + */ + private static void updateGamePieces() { + updateGamePiecesPeriodically(); + updateCollection(); + updateEjection(); + updateHeldGamePiecePoses(); + } + + /** + * Logs the position of all the game pieces. + */ + private static void logGamePieces() { + Logger.recordOutput("Poses/GamePieces/GamePieces", mapSimulatedGamePieceListToPoseArray(GAME_PIECES_ON_FIELD)); + } + + private static void updateGamePiecesPeriodically() { + for (SimulatedGamePiece gamePiece : GAME_PIECES_ON_FIELD) + gamePiece.updatePeriodically(HELD_GAME_PIECE_INDEX != null && HELD_GAME_PIECE_INDEX == GAME_PIECES_ON_FIELD.indexOf(gamePiece)); + } + + private static void updateCollection() { + final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + final Pose3d robotRelativeCollectionPose = new Pose3d(); + final Pose3d collectionPose = robotPose.plus(toTransform(robotRelativeCollectionPose)); + + updateFeederCollection(robotPose); + + if (isCollectingGamePiece() && HELD_GAME_PIECE_INDEX == null) + HELD_GAME_PIECE_INDEX = getIndexOfCollectedGamePiece(collectionPose, GAME_PIECES_ON_FIELD, SimulatedGamePieceConstants.INTAKE_TOLERANCE_METERS); + } + + private static void updateFeederCollection(Pose3d robotPose) { + final double distanceFromFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.FEEDER_POSITION.get()); + + if (isCollectingGamePieceFromFeeder() && HELD_GAME_PIECE_INDEX == null && + distanceFromFeeder < SimulatedGamePieceConstants.FEEDER_INTAKE_TOLERANCE_METERS) { + GAME_PIECES_ON_FIELD.add(new SimulatedGamePiece(new Pose3d(), SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE)); + HELD_GAME_PIECE_INDEX = GAME_PIECES_ON_FIELD.size() - 1; + } + } + + /** + * Gets the index of the game piece that is being collected. + * + * @param collectionPose the pose of the collection mechanism + * @param gamePieceList the list of game pieces + * @return the index of the game piece that is being collected + */ + private static Integer getIndexOfCollectedGamePiece(Pose3d collectionPose, ArrayList gamePieceList, double intakeTolerance) { + for (SimulatedGamePiece gamePiece : gamePieceList) + if (gamePiece.getDistanceFromPoseMeters(collectionPose) <= intakeTolerance) + return gamePieceList.indexOf(gamePiece); + return null; + } + + private static boolean isCollectingGamePiece() { + return false;//TODO: Implement + } + + private static boolean isCollectingGamePieceFromFeeder() { + return false;//TODO: Implement + } + + private static void updateEjection() { + if (HELD_GAME_PIECE_INDEX != null && isEjectingGamePiece()) { + final SimulatedGamePiece heldGamePiece = GAME_PIECES_ON_FIELD.get(HELD_GAME_PIECE_INDEX); + ejectGamePiece(heldGamePiece); + HELD_GAME_PIECE_INDEX = null; + } + } + + private static void ejectGamePiece(SimulatedGamePiece ejectedGamePiece) { + final Translation3d robotSelfRelativeVelocity = new Translation3d(RobotContainer.SWERVE.getSelfRelativeVelocity()); + final Translation3d robotRelativeReleaseVelocity = new Translation3d();//TODO:This should be extracted to a method in a different branch... + + ejectedGamePiece.release(robotSelfRelativeVelocity.plus(robotRelativeReleaseVelocity).rotateBy(new Rotation3d(RobotContainer.SWERVE.getHeading()))); + } + + private static boolean isEjectingGamePiece() { + return false;//TODO: Implement + } + + /** + * Updates the position of the held game pieces so that they stay inside the robot. + */ + private static void updateHeldGamePiecePoses() { + final Pose3d robotRelativeHeldGamePiecePosition = new Pose3d(); + updateHeldGamePiecePose(robotRelativeHeldGamePiecePosition, GAME_PIECES_ON_FIELD, HELD_GAME_PIECE_INDEX); + } + + private static void updateHeldGamePiecePose(Pose3d robotRelativeHeldGamePiecePose, ArrayList gamePieceList, Integer heldGamePieceIndex) { + if (heldGamePieceIndex == null) + return; + + final SimulatedGamePiece heldGamePiece = gamePieceList.get(heldGamePieceIndex); + heldGamePiece.updatePose(calculateHeldGamePieceFieldRelativePose(robotRelativeHeldGamePiecePose)); + } + + /** + * Calculate the position of the held game piece relative to the field. + * + * @param robotRelativeHeldGamePiecePosition the position of the held game piece relative to the robot + * @return the position of the held game piece relative to the field + */ + private static Pose3d calculateHeldGamePieceFieldRelativePose(Pose3d robotRelativeHeldGamePiecePosition) { + final Pose3d robotPose3d = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + return robotPose3d.plus(toTransform(robotRelativeHeldGamePiecePosition)); + } + + /** + * Converts a Pose3d into a Transform3d. + * + * @param pose the target Pose3d + * @return the Transform3d + */ + private static Transform3d toTransform(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + private static Pose3d[] mapSimulatedGamePieceListToPoseArray(ArrayList gamePieces) { + final Pose3d[] poses = new Pose3d[gamePieces.size()]; + for (int i = 0; i < poses.length; i++) + poses[i] = gamePieces.get(i).getPose(); + return poses; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java new file mode 100644 index 00000000..156cd2f8 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.misc.simulatedfield; + +import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.lib.utilities.flippable.FlippablePose3d; + +import java.util.ArrayList; +import java.util.List; + +public class SimulationScoringHandler { + public static void checkGamePieceScored(SimulatedGamePiece gamePiece) { + final ArrayList scoreLocations = new ArrayList<>(List.of(SimulatedGamePieceConstants.SCORING_LOCATION)); + for (FlippablePose3d scoreLocation : scoreLocations) { + final Pose3d flippedPose = scoreLocation.get(); + if (isGamePieceScored(gamePiece, flippedPose, SimulatedGamePieceConstants.SCORING_TOLERANCE_METERS)) { + gamePiece.isScored = true; + gamePiece.updatePose(flippedPose); + scoreLocations.remove(scoreLocation); + return; + } + } + } + + private static boolean isGamePieceScored(SimulatedGamePiece gamePiece, Pose3d scoreLocation, double scoringToleranceMeters) { + final double distanceFromScoreZoneMeters = gamePiece.getDistanceFromPoseMeters(scoreLocation); + return distanceFromScoreZoneMeters < scoringToleranceMeters; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java new file mode 100644 index 00000000..959bf750 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -0,0 +1,184 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; +import org.littletonrobotics.junction.Logger; + +/** + * AprilTagCamera is an object that provides the robot's pose from a camera using one or multiple april tags. + * An apriltag is a 2d QR-code used to find the robot's position on the field. + * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. + */ +public class AprilTagCamera { + protected final String name; + private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); + private final Transform2d cameraToRobotCenter; + private final StandardDeviations standardDeviations; + private final AprilTagCameraIO aprilTagCameraIO; + private Pose2d estimatedRobotPose = new Pose2d(); + + /** + * Constructs a new AprilTagCamera. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera. + * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, + * to avoid more inaccuracies like pitch and roll. + * The reset will be used for creating a camera in simulation + * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, + * will be changed as the distance from the tag(s) changes and the number of tags changes + */ + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, + String name, Transform3d robotCenterToCamera, + StandardDeviations standardDeviations) { + this.name = name; + this.standardDeviations = standardDeviations; + this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse(); + + aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, robotCenterToCamera); + } + + public void update() { + aprilTagCameraIO.updateInputs(inputs); + Logger.processInputs("Cameras/" + name, inputs); + + estimatedRobotPose = calculateRobotPose(); + logCameraInfo(); + } + + public Pose2d getEstimatedRobotPose() { + return estimatedRobotPose; + } + + public String getName() { + return name; + } + + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; + } + + public boolean hasValidResult() { + return inputs.hasResult && + inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && + inputs.distancesFromTags[0] < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_RESULT_METERS; + } + + /** + * Solve PNP is inaccurate the further the camera is from the tag. + * Because of this, there are some things we might want to do only if we are close enough to get an accurate enough result. + * This method checks if the current distance from the tag is less than the maximum distance for an accurate result, which is defined as the variable {@link AprilTagCameraConstants#MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS}. + * + * @return if the camera is close enough to the tag to get an accurate result from solve PNP + */ + public boolean isWithinBestTagRangeForAccurateSolvePNPResult() { + return hasValidResult() && inputs.distancesFromTags[0] < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS; + } + + /** + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calibrated gain. + * + * @return the standard deviations of the current estimated pose + */ + public StandardDeviations calculateStandardDeviations() { + final double averageDistanceFromTags = calculateAverageDistanceFromTags(); + final double translationStandardDeviation = calculateStandardDeviation(standardDeviations.translationStandardDeviation(), averageDistanceFromTags, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = calculateStandardDeviation(standardDeviations.thetaStandardDeviation(), averageDistanceFromTags, inputs.visibleTagIDs.length); + + Logger.recordOutput("StandardDeviations/" + name + "/translations", translationStandardDeviation); + Logger.recordOutput("StandardDeviations/" + name + "/theta", thetaStandardDeviation); + + return new StandardDeviations(translationStandardDeviation, thetaStandardDeviation); + } + + private Pose2d calculateRobotPose() { + if (!hasValidResult()) + return null; + + return chooseBestRobotPose(); + } + + private Pose2d chooseBestRobotPose() { + if (!inputs.hasConstrainedResult || isWithinBestTagRangeForAccurateSolvePNPResult()) + return chooseBestNormalSolvePNPPose(); + + return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d()); + } + + private Pose2d chooseBestNormalSolvePNPPose() { + final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d()); + + if (inputs.bestCameraSolvePNPPose.equals(inputs.alternateCameraSolvePNPPose)) + return bestPose; + if (inputs.alternateCameraSolvePNPPose.getTranslation().toTranslation2d().getDistance(FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getTranslation().toTranslation2d()) < 0.1 || DriverStation.isDisabled()) + return bestPose; + + final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d()); + final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation(); + + final double bestAngleDifference = Math.abs(bestPose.getRotation().minus(robotAngleAtResultTime).getRadians()); + final double alternateAngleDifference = Math.abs(alternatePose.getRotation().minus(robotAngleAtResultTime).getRadians()); + + return bestAngleDifference > alternateAngleDifference ? alternatePose : bestPose; + } + + private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) { + return cameraPose.transformBy(cameraToRobotCenter); + } + + /** + * Calculates an aspect of the standard deviations of the estimated pose using a formula. + * As we get further from the tag(s), this will return a less trusting (higher deviation) result. + * + * @param exponent a calibrated gain + * @param distance the distance from the tag(s) + * @param numberOfVisibleTags the number of visible tags + * @return the standard deviation + */ + private double calculateStandardDeviation(double exponent, double distance, int numberOfVisibleTags) { + return exponent * (distance * distance) / numberOfVisibleTags; + } + + private void logCameraInfo() { + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + + if (hasValidResult()) { + Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", new Pose2d[]{estimatedRobotPose}); + return; + } + + Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_ARRAY); + } + + private void logUsedTags() { + if (!hasValidResult()) { + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); + return; + } + + final Pose3d[] usedTagPoses = isWithinBestTagRangeForAccurateSolvePNPResult() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; + for (int i = 0; i < usedTagPoses.length; i++) + usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); + Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); + } + + private Transform2d toTransform2d(Transform3d transform3d) { + final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d(); + final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d(); + + return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation); + } + + private double calculateAverageDistanceFromTags() { + double totalDistance = 0; + for (int visibleTagID : inputs.visibleTagIDs) { + totalDistance += FieldConstants.TAG_ID_TO_POSE.get(visibleTagID).getTranslation().getDistance(inputs.bestCameraSolvePNPPose.getTranslation()); + } + return totalDistance / inputs.visibleTagIDs.length; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java new file mode 100644 index 00000000..bd693b84 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -0,0 +1,66 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import frc.trigon.lib.hardware.RobotHardwareStats; + +import java.util.function.BiFunction; + +public class AprilTagCameraConstants { + static final double + MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_RESULT_METERS = 5, + MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2; + static final Pose2d[] EMPTY_POSE_ARRAY = new Pose2d[0]; + static final double MAXIMUM_AMBIGUITY = 0.4; + public static final PhotonPoseEstimator.ConstrainedSolvepnpParams CONSTRAINED_SOLVE_PNP_PARAMS = new PhotonPoseEstimator.ConstrainedSolvepnpParams(false, 0.1); + + public static final VisionSystemSim VISION_SIMULATION = RobotHardwareStats.isSimulation() ? new VisionSystemSim("VisionSimulation") : null; + private static final int + SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, + SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, + SIMULATION_CAMERA_FPS = 60, + SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, + SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; + private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70); + private static final double + SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25, + SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); + + static { + if (RobotHardwareStats.isSimulation()) { + configureSimulationCameraProperties(); + VISION_SIMULATION.addAprilTags(FieldConstants.APRIL_TAG_FIELD_LAYOUT); + } + } + + private static void configureSimulationCameraProperties() { + SIMULATION_CAMERA_PROPERTIES.setCalibration(SIMULATION_CAMERA_RESOLUTION_WIDTH, SIMULATION_CAMERA_RESOLUTION_HEIGHT, SIMULATION_CAMERA_FOV); + SIMULATION_CAMERA_PROPERTIES.setCalibError(SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR, SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS); + SIMULATION_CAMERA_PROPERTIES.setFPS(SIMULATION_CAMERA_FPS); + SIMULATION_CAMERA_PROPERTIES.setAvgLatencyMs(SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setLatencyStdDevMs(SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setExposureTimeMs(SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS); + } + + public enum AprilTagCameraType { + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + SIMULATION_CAMERA(AprilTagSimulationCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); + + final BiFunction createIOFunction; + + AprilTagCameraType(BiFunction createIOFunction) { + this.createIOFunction = createIOFunction; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java new file mode 100644 index 00000000..8f3462d2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -0,0 +1,33 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.littletonrobotics.junction.AutoLog; +import frc.trigon.lib.hardware.RobotHardwareStats; + +public class AprilTagCameraIO { + static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotToCamera) { + if (RobotHardwareStats.isReplay()) + return new AprilTagCameraIO(); + if (RobotHardwareStats.isSimulation()) + aprilTagCameraType = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA; + + return aprilTagCameraType.createIOFunction.apply(name, robotToCamera); + } + + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + } + + @AutoLog + public static class AprilTagCameraInputs { + public boolean hasResult = false; + public boolean hasConstrainedResult = false; + public double latestResultTimestampSeconds = 0; + public Pose3d bestCameraSolvePNPPose = new Pose3d(); + public Pose3d alternateCameraSolvePNPPose = new Pose3d(); + public Pose3d constrainedSolvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; + public double poseAmbiguity = 1; + public double[] distancesFromTags = new double[0]; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java new file mode 100644 index 00000000..60a0e81b --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -0,0 +1,63 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.lib.utilities.LimelightHelpers; + +// TODO: Fully implement this class, Limelight currently not supported. +public class AprilTagLimelightIO extends AprilTagCameraIO { + private final String hostname; + + public AprilTagLimelightIO(String hostname, Transform3d robotToCamera) { + this.hostname = hostname; + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + + inputs.hasResult = results.targets_Fiducials.length > 0; + + if (inputs.hasResult) { + updateHasTargetInputs(inputs, results); + return; + } + updateNoTargetInputs(inputs); + } + + private void updateHasTargetInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { + inputs.bestCameraSolvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; + inputs.visibleTagIDs = getVisibleTagIDs(results); + } + + private void updateNoTargetInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.bestCameraSolvePNPPose = new Pose3d(); + inputs.visibleTagIDs = new int[0]; + } + + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { + return results.targets_Fiducials[0]; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java new file mode 100644 index 00000000..138968e4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -0,0 +1,188 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.estimation.VisionEstimation; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.PnpResult; + +import java.util.Arrays; +import java.util.List; +import java.util.Optional; + +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { + private final Transform3d robotToCamera; + final PhotonCamera photonCamera; + + public AprilTagPhotonCameraIO(String cameraName, Transform3d robotToCamera) { + photonCamera = new PhotonCamera(cameraName); + this.robotToCamera = robotToCamera; + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = getLatestPipelineResult(); + + inputs.hasResult = latestResult != null && latestResult.hasTargets(); + if (inputs.hasResult) { + updateHasTargetInputs(inputs, latestResult); + return; + } + + updateNoTargetInputs(inputs); + } + + private PhotonPipelineResult getLatestPipelineResult() { + final List unreadResults = photonCamera.getAllUnreadResults(); + return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); + } + + private void updateHasTargetInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final PhotonTrackedTarget[] visibleNotHatefulTags = getVisibleNotHatefulTags(latestResult); + final PhotonTrackedTarget bestTag = visibleNotHatefulTags.length == 0 ? null : visibleNotHatefulTags[0]; + if (bestTag == null) { + updateNoTargetInputs(inputs); + inputs.hasResult = false; + return; + } + + updateSolvePNPPoses(inputs, latestResult, bestTag); + if (inputs.bestCameraSolvePNPPose == null) { + updateNoTargetInputs(inputs); + inputs.hasResult = false; + return; + } + + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.visibleTagIDs = getVisibleTagIDs(visibleNotHatefulTags); + inputs.poseAmbiguity = latestResult.getMultiTagResult().isPresent() ? 0 : bestTag.getPoseAmbiguity(); + inputs.distancesFromTags = getDistancesFromTags(visibleNotHatefulTags); + inputs.hasConstrainedResult = false; + } + + private void updateNoTargetInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.bestCameraSolvePNPPose = new Pose3d(); + inputs.alternateCameraSolvePNPPose = inputs.bestCameraSolvePNPPose; + inputs.constrainedSolvePNPPose = inputs.alternateCameraSolvePNPPose; + inputs.visibleTagIDs = new int[0]; + inputs.hasConstrainedResult = false; + } + + private PhotonTrackedTarget[] getVisibleNotHatefulTags(PhotonPipelineResult result) { + final List targets = result.getTargets(); + final PhotonTrackedTarget[] visibleTagIDs = new PhotonTrackedTarget[targets.size()]; + int index = 0; + + for (PhotonTrackedTarget target : targets) { + if (FieldConstants.TAG_ID_TO_POSE.containsKey(target.getFiducialId())) { + visibleTagIDs[index] = target; + index++; + } + } + + return Arrays.copyOf(visibleTagIDs, index); + } + + private void updateSolvePNPPoses(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult, PhotonTrackedTarget bestTag) { + if (latestResult.getMultiTagResult().isPresent()) { + final Transform3d tagToCamera = latestResult.getMultiTagResult().get().estimatedPose.best; + inputs.bestCameraSolvePNPPose = FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin().transformBy(tagToCamera); + inputs.alternateCameraSolvePNPPose = inputs.bestCameraSolvePNPPose; + return; + } + + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTag.getFiducialId()); + if (tagPose == null) { + inputs.bestCameraSolvePNPPose = null; + return; + } + + final Transform3d bestTagToCamera = bestTag.getBestCameraToTarget().inverse(); + final Transform3d alternateTagToCamera = bestTag.getAlternateCameraToTarget().inverse(); + + inputs.bestCameraSolvePNPPose = tagPose.transformBy(bestTagToCamera); + inputs.alternateCameraSolvePNPPose = tagPose.transformBy(alternateTagToCamera); + +// updateConstrainedSolvePNPPose(inputs, latestResult); + } + + private void updateConstrainedSolvePNPPose(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Pose3d constrainedSolvePNPPose = calculateConstrainedSolvePNPPose(latestResult, inputs.bestCameraSolvePNPPose); + if (constrainedSolvePNPPose == null) { + inputs.hasConstrainedResult = false; + return; + } + + inputs.constrainedSolvePNPPose = constrainedSolvePNPPose; + inputs.hasConstrainedResult = true; + } + + private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pose3d bestCameraSolvePNPPose) { + final Optional> cameraMatrix = photonCamera.getCameraMatrix(); + final Optional> distCoeffs = photonCamera.getDistCoeffs(); + + if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) + return null; + + Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(this.robotToCamera.inverse()); + final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(result.getTimestampSeconds()).getRotation(); + + if (!AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree()) { + fieldToRobotSeed = new Pose3d( + fieldToRobotSeed.getTranslation(), + new Rotation3d(robotYawAtTimestamp) + ); + } + + final Optional pnpResult = VisionEstimation.estimateRobotPoseConstrainedSolvepnp( + cameraMatrix.get(), + distCoeffs.get(), + result.getTargets(), + robotToCamera, + fieldToRobotSeed, + FieldConstants.APRIL_TAG_FIELD_LAYOUT, + TargetModel.kAprilTag36h11, + AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree(), + robotYawAtTimestamp, + AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingScaleFactor() + ); + + return pnpResult.map(value -> Pose3d.kZero.plus(value.best).transformBy(robotToCamera)).orElse(null); + } + + private int[] getVisibleTagIDs(PhotonTrackedTarget[] targets) { + final int[] visibleTagIDs = new int[targets.length]; + + for (int i = 0; i < visibleTagIDs.length; i++) + visibleTagIDs[i] = targets[i].getFiducialId(); + + return visibleTagIDs; + } + + private double[] getDistancesFromTags(PhotonTrackedTarget[] targets) { + final double[] distances = new double[targets.length]; + + for (int i = 0; i < targets.length; i++) + distances[i] = getDistanceFromTarget(targets[i]); + + return distances; + } + + private double getDistanceFromTarget(PhotonTrackedTarget target) { + return target.getBestCameraToTarget().getTranslation().getNorm(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java new file mode 100644 index 00000000..c68ef0e0 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -0,0 +1,16 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import org.photonvision.simulation.PhotonCameraSim; + +public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { + + public AprilTagSimulationCameraIO(String cameraName, Transform3d robotToCamera) { + super(cameraName, robotToCamera); + + final PhotonCameraSim cameraSimulation = new PhotonCameraSim(photonCamera, AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES); + cameraSimulation.enableDrawWireframe(true); + AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSimulation, robotToCamera); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java new file mode 100644 index 00000000..b549a358 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -0,0 +1,287 @@ +package frc.trigon.robot.poseestimation.poseestimator; + +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSource; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceConstants; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; +import frc.trigon.lib.utilities.QuickSortHandler; +import frc.trigon.lib.utilities.flippable.Flippable; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import java.util.Arrays; +import java.util.Map; + +/** + * A class that estimates the robot's pose using WPILib's {@link SwerveDrivePoseEstimator} and {@link SwerveDriveOdometry}. + */ +public class PoseEstimator implements AutoCloseable { + private final SwerveDrivePoseEstimator swerveDrivePoseEstimator = createSwerveDrivePoseEstimator(); + private final SwerveDriveOdometry swerveDriveOdometry = createSwerveDriveOdometry(); + private final Field2d field = new Field2d(); + private final AprilTagCamera[] aprilTagCameras; + private final RelativeRobotPoseSource relativeRobotPoseSource; + private final boolean shouldUseRelativeRobotPoseSource; + + /** + * Constructs a new PoseEstimator and sets the relativeRobotPoseSource. + * This constructor enables usage of a relative robot pose source and disables the use of april tags for pose estimation, and instead uses them to reset the relative robot pose source's offset. + * + * @param relativeRobotPoseSource the relative robot pose source that should be used to update the pose estimator + * @param aprilTagCameras the apriltag cameras that should be used to update the relative robot pose source + */ + public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; + this.relativeRobotPoseSource = relativeRobotPoseSource; + this.shouldUseRelativeRobotPoseSource = true; + + initialize(); + } + + /** + * Constructs a new PoseEstimator. + * This constructor disables the use of a relative robot pose source and instead uses april tags cameras for pose estimation. + * + * @param aprilTagCameras the cameras that should be used to update the pose estimator + */ + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; + this.relativeRobotPoseSource = null; + this.shouldUseRelativeRobotPoseSource = false; + + initialize(); + } + + @Override + public void close() { + field.close(); + } + + public void periodic() { + if (shouldUseRelativeRobotPoseSource) + updateFromRelativeRobotPoseSource(); + else + updateFromAprilTagCameras(); + + field.setRobotPose(getEstimatedRobotPose()); + } + + public void resetHeading() { + final Rotation2d resetRotation = Flippable.isRedAlliance() ? Rotation2d.k180deg : Rotation2d.kZero; + swerveDrivePoseEstimator.resetRotation(resetRotation); + swerveDriveOdometry.resetRotation(resetRotation); + } + + /** + * Resets the pose estimator to the given pose, and the gyro to the given pose's heading. + * + * @param newPose the pose to reset to, relative to the blue alliance's driver station right corner + */ + public void resetPose(Pose2d newPose) { + RobotContainer.SWERVE.setHeading(newPose.getRotation()); + + swerveDrivePoseEstimator.resetPose(newPose); + swerveDriveOdometry.resetPose(newPose); + if (shouldUseRelativeRobotPoseSource) + relativeRobotPoseSource.resetOffset(newPose); + } + + public void resetOdometry() { + swerveDriveOdometry.resetPose(getEstimatedRobotPose()); + } + + /** + * @return the estimated pose of the robot, relative to the blue alliance's driver station right corner + */ + @AutoLogOutput(key = "Poses/Robot/PoseEstimator/EstimatedRobotPose") + public Pose2d getEstimatedRobotPose() { + return swerveDrivePoseEstimator.getEstimatedPosition(); + } + + /** + * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner + */ + @AutoLogOutput(key = "Poses/Robot/PoseEstimator/EstimatedOdometryPose") + public Pose2d getEstimatedOdometryPose() { + return swerveDriveOdometry.getPoseMeters(); + } + + /** + * Updates the pose estimator with the given swerve wheel positions and gyro rotations. + * This function accepts an array of swerve wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz). + * This means you could have a couple of odometry updates per main loop, and you would want to update the pose estimator with all of them. + * + * @param swerveWheelPositions the swerve wheel positions accumulated since the last update + * @param gyroRotations the gyro rotations accumulated since the last update + */ + public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPositions, Rotation2d[] gyroRotations, double[] timestamps) { + for (int i = 0; i < swerveWheelPositions.length; i++) { + swerveDrivePoseEstimator.updateWithTime(timestamps[i], gyroRotations[i], swerveWheelPositions[i]); + swerveDriveOdometry.update(gyroRotations[i], swerveWheelPositions[i]); + } + } + + /** + * Gets the estimated pose of the robot at the target timestamp. + * Unlike {@link #getPredictedRobotFuturePose} which predicts a future pose, this gets a stored pose from the estimator's buffer. + * + * @param timestamp the Rio's FPGA timestamp + * @return the robot's estimated pose at the timestamp + */ + public Pose2d samplePoseAtTimestamp(double timestamp) { + return swerveDrivePoseEstimator.sampleAt(timestamp).orElse(null); + } + + /** + * Predicts the robot's pose after the specified time. + * Unlike {@link #samplePoseAtTimestamp(double)} which gets a previous pose from the buffer, this predicts the future pose of the robot. + * + * @param seconds the number of seconds into the future to predict the robot's pose for + * @return the predicted pose + */ + public Pose2d getPredictedRobotPose(double seconds) { + final ChassisSpeeds robotVelocity = RobotContainer.SWERVE.getSelfRelativeChassisSpeeds(); + final double predictedX = robotVelocity.vxMetersPerSecond * seconds; + final double predictedY = robotVelocity.vyMetersPerSecond * seconds; + final Rotation2d predictedRotation = Rotation2d.fromRadians(robotVelocity.omegaRadiansPerSecond * seconds); + return getEstimatedRobotPose().transformBy(new Transform2d(predictedX, predictedY, predictedRotation)); + } + + private void initialize() { + putAprilTagsOnFieldWidget(); + SmartDashboard.putData("Field", field); + logTargetPath(); + } + + private void putAprilTagsOnFieldWidget() { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { + final Pose2d tagPose = entry.getValue().toPose2d(); + field.getObject("Tag " + entry.getKey()).setPose(tagPose); + } + } + + /** + * Logs and updates the field widget with the target PathPlanner path as an array of Pose2ds. + */ + private void logTargetPath() { + PathPlannerLogging.setLogActivePathCallback((pathPoses) -> { + field.getObject("path").setPoses(pathPoses); + Logger.recordOutput("PathPlanner/Path", pathPoses.toArray(new Pose2d[0])); + }); + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + RobotContainer.SWERVE.setTargetPathPlannerPose(pose); + Logger.recordOutput("PathPlanner/TargetPose", pose); + }); + } + + private void updateFromRelativeRobotPoseSource() { + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + aprilTagCamera.update(); + + if (aprilTagCamera.isWithinBestTagRangeForAccurateSolvePNPResult() && isUnderMaximumSpeedForOffsetResetting()) + relativeRobotPoseSource.resetOffset(aprilTagCamera.getEstimatedRobotPose()); + } + + relativeRobotPoseSource.update(); + + if (relativeRobotPoseSource.hasNewResult()) { + swerveDrivePoseEstimator.addVisionMeasurement( + relativeRobotPoseSource.getEstimatedRobotPose(), + relativeRobotPoseSource.getLatestResultTimestampSeconds(), + RelativeRobotPoseSourceConstants.STANDARD_DEVIATIONS.toMatrix() + ); + } + } + + private void updateFromAprilTagCameras() { + final AprilTagCamera[] newResultCameras = getCamerasWithResults(); +// sortCamerasByLatestResultTimestamp(newResultCameras); + + for (AprilTagCamera aprilTagCamera : newResultCameras) { + swerveDrivePoseEstimator.addVisionMeasurement( + aprilTagCamera.getEstimatedRobotPose(), + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.calculateStandardDeviations().toMatrix() + ); + } + } + + /** + * Checks if the current velocity of the slow enough to get an accurate enough result to reset the offset of the {@link RelativeRobotPoseSource}. + * + * @return if the robot is moving slow enough to calculate an accurate offset result. + */ + private boolean isUnderMaximumSpeedForOffsetResetting() { + final ChassisSpeeds chassisSpeeds = RobotContainer.SWERVE.getSelfRelativeChassisSpeeds(); + final double currentTranslationVelocityMetersPerSecond = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + final double currentThetaVelocityRadiansPerSecond = chassisSpeeds.omegaRadiansPerSecond; + return currentTranslationVelocityMetersPerSecond <= PoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && + currentThetaVelocityRadiansPerSecond <= PoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; + } + + private AprilTagCamera[] getCamerasWithResults() { + final AprilTagCamera[] camerasWithNewResult = new AprilTagCamera[aprilTagCameras.length]; + int index = 0; + + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + aprilTagCamera.update(); + if (aprilTagCamera.hasValidResult() && aprilTagCamera.getEstimatedRobotPose() != null) { + camerasWithNewResult[index] = aprilTagCamera; + index++; + } + } + + return Arrays.copyOf(camerasWithNewResult, index); + } + + private void sortCamerasByLatestResultTimestamp(AprilTagCamera[] aprilTagCameras) { + QuickSortHandler.sort(aprilTagCameras, AprilTagCamera::getLatestResultTimestampSeconds); + } + + private SwerveDriveOdometry createSwerveDriveOdometry() { + final SwerveModulePosition[] swerveModulePositions = { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + return new SwerveDriveOdometry( + SwerveConstants.KINEMATICS, + new Rotation2d(), + swerveModulePositions + ); + } + + private SwerveDrivePoseEstimator createSwerveDrivePoseEstimator() { + final SwerveModulePosition[] swerveModulePositions = { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + return new SwerveDrivePoseEstimator( + SwerveConstants.KINEMATICS, + new Rotation2d(), + swerveModulePositions, + new Pose2d(), + PoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), + VecBuilder.fill(0, 0, 0) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java new file mode 100644 index 00000000..d341b294 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -0,0 +1,11 @@ +package frc.trigon.robot.poseestimation.poseestimator; + +public class PoseEstimatorConstants { + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; + + static final StandardDeviations ODOMETRY_STANDARD_DEVIATIONS = new StandardDeviations(0.003, 0.0002); + static final double + MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND = 0, + MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND = 0; +} + diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java new file mode 100644 index 00000000..bbe11c6c --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java @@ -0,0 +1,31 @@ +package frc.trigon.robot.poseestimation.poseestimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * Standard Deviations represent how ambiguous an estimated pose is using calibrated gains. + * The higher the value is, the more ambiguous the pose is and the less trustworthy the result. + * Standard Deviations are used to reduce noise in a pose estimate result by accounting for how much each result is wrong by. + */ +public record StandardDeviations(double translationStandardDeviation, double thetaStandardDeviation) { + /** + * Constructs an object that stores how ambiguous the estimated pose of the robot is. + * The greater the number, the less trustworthy the pose is. + * + * @param translationStandardDeviation the ambiguity of the translation aspect of the pose estimation + * @param thetaStandardDeviation the ambiguity of the rotation aspect of the pose estimation + */ + public StandardDeviations { + } + + public Matrix toMatrix() { + return VecBuilder.fill( + translationStandardDeviation, + translationStandardDeviation, + thetaStandardDeviation + ); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java new file mode 100644 index 00000000..0d1e1b9f --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java @@ -0,0 +1,72 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import org.littletonrobotics.junction.AutoLogOutput; + +/** + * A relative robot pose source is a robot pose source that calculates its position externally. + * The origin point of the relative pose source doesn't necessarily match the origin point of the robot's estimated pose, so the estimated pose needs to be transformed by that difference. + */ +public class RelativeRobotPoseSource { + private final RelativeRobotPoseSourceInputsAutoLogged inputs = new RelativeRobotPoseSourceInputsAutoLogged(); + private final Transform2d cameraToRobotCenter; + private final RelativeRobotPoseSourceIO relativeRobotPoseSourceIO; + + private double lastResultTimestampSeconds = 0; + private Pose2d robotPoseAtSyncTime = new Pose2d(); + private Pose2d relativePoseSourceEstimatedPoseAtSyncTime = new Pose2d(); + + /** + * Constructs a new RelativeRobotPoseSource. + * + * @param cameraToRobotCenter the transform of the camera to the robot's origin point + * @param hostname the name of the camera in the NetworkTables + */ + public RelativeRobotPoseSource(Transform2d cameraToRobotCenter, String hostname) { + this.cameraToRobotCenter = cameraToRobotCenter; + this.relativeRobotPoseSourceIO = RelativeRobotPoseSourceIO.generateIO(hostname); + } + + public void update() { + relativeRobotPoseSourceIO.updateInputs(inputs); + } + + /** + * Resets the offset from the relative robot pose source to the robot pose. + * The offset is stored as two {@link Pose2d}s, one for the robot's pose and the other for the relative pose source's pose both at the same timestamp. + * With these two poses, we can calculate the total distance moved from the relative pose source's perspective and transform the robot's pose by the same amount. + * + * @param robotPose the current pose of the robot + */ + public void resetOffset(Pose2d robotPose) { + this.robotPoseAtSyncTime = robotPose; + this.relativePoseSourceEstimatedPoseAtSyncTime = getRobotPoseFromCameraPose(inputs.pose); + } + + @AutoLogOutput(key = "RelativeRobotPoseSource/EstimatedRobotPose") + public Pose2d getEstimatedRobotPose() { + final Transform2d movementFromSyncedPose = new Transform2d(relativePoseSourceEstimatedPoseAtSyncTime, getRobotPoseFromCameraPose(inputs.pose)); + return robotPoseAtSyncTime.transformBy(movementFromSyncedPose); + } + + public double getLatestResultTimestampSeconds() { + return inputs.resultTimestampSeconds; + } + + public boolean hasNewResult() { + return inputs.hasResult && isNewTimestamp(); + } + + private boolean isNewTimestamp() { + if (lastResultTimestampSeconds == getLatestResultTimestampSeconds()) + return false; + + lastResultTimestampSeconds = getLatestResultTimestampSeconds(); + return true; + } + + private Pose2d getRobotPoseFromCameraPose(Pose2d cameraPose) { + return cameraPose.transformBy(cameraToRobotCenter); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java new file mode 100644 index 00000000..7c7a2812 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java @@ -0,0 +1,7 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; + +public class RelativeRobotPoseSourceConstants { + public static final StandardDeviations STANDARD_DEVIATIONS = new StandardDeviations(0.0000001, 0.0000001); +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java new file mode 100644 index 00000000..9c30de39 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java @@ -0,0 +1,29 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceSimulationIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceT265IO; +import org.littletonrobotics.junction.AutoLog; +import frc.trigon.lib.hardware.RobotHardwareStats; + +public class RelativeRobotPoseSourceIO { + static RelativeRobotPoseSourceIO generateIO(String hostname) { + if (RobotHardwareStats.isReplay()) + return new RelativeRobotPoseSourceIO(); + if (RobotHardwareStats.isSimulation()) + return new RelativeRobotPoseSourceSimulationIO(); + return new RelativeRobotPoseSourceT265IO(hostname); + } + + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + } + + @AutoLog + public static class RelativeRobotPoseSourceInputs { + public int framesPerSecond = 0; + public double batteryPercentage = 0; + public Pose2d pose = new Pose2d(); + public double resultTimestampSeconds = 0; + public boolean hasResult = false; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java new file mode 100644 index 00000000..ca0dad31 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java @@ -0,0 +1,17 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource.io; + +import edu.wpi.first.wpilibj.Timer; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceInputsAutoLogged; + +public class RelativeRobotPoseSourceSimulationIO extends RelativeRobotPoseSourceIO { + @Override + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + inputs.framesPerSecond = 60; + inputs.batteryPercentage = 100; + inputs.pose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedOdometryPose(); + inputs.resultTimestampSeconds = Timer.getTimestamp(); + inputs.hasResult = true; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java new file mode 100644 index 00000000..f2c5d224 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java @@ -0,0 +1,67 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource.io; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceInputsAutoLogged; +import frc.trigon.lib.utilities.JsonHandler; + +public class RelativeRobotPoseSourceT265IO extends RelativeRobotPoseSourceIO { + private final NetworkTableEntry jsonDumpEntry; + + public RelativeRobotPoseSourceT265IO(String hostname) { + jsonDumpEntry = NetworkTableInstance.getDefault().getTable(hostname).getEntry("JsonDump"); + } + + @Override + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + final T265JsonDump jsonDump = readJsonDump(); + if (jsonDump == null) { + updateNoResultInputs(inputs); + return; + } + updateHasResultInputs(inputs, jsonDump); + } + + private void updateNoResultInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + inputs.framesPerSecond = 0; + inputs.hasResult = false; + } + + private void updateHasResultInputs(RelativeRobotPoseSourceInputsAutoLogged inputs, T265JsonDump jsonDump) { + inputs.framesPerSecond = jsonDump.framesPerSecond; + inputs.batteryPercentage = jsonDump.batteryPercentage; + inputs.pose = extractPose(jsonDump); + inputs.resultTimestampSeconds = jsonDumpEntry.getLastChange(); + inputs.hasResult = true; + } + + private T265JsonDump readJsonDump() { + return JsonHandler.parseJsonStringToObject(jsonDumpEntry.getString(""), T265JsonDump.class); + } + + private Pose2d extractPose(T265JsonDump jsonDump) { + final Translation2d translation = extractTranslation(jsonDump); + final Rotation2d heading = extractHeading(jsonDump); + return new Pose2d(translation, heading); + } + + private Translation2d extractTranslation(T265JsonDump jsonDump) { + return new Translation2d(jsonDump.xPositionMeters, jsonDump.yPositionMeters); + } + + private Rotation2d extractHeading(T265JsonDump jsonDump) { + return Rotation2d.fromRadians(jsonDump.rotationRadians); + } + + private static class T265JsonDump { + private int framesPerSecond = 0; + private double batteryPercentage = 0; + private double xPositionMeters = 0; + private double yPositionMeters = 0; + private double rotationRadians = 0; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java new file mode 100644 index 00000000..b3fc0856 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -0,0 +1,171 @@ +package frc.trigon.robot.subsystems; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; +import frc.trigon.lib.hardware.RobotHardwareStats; + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.Executor; +import java.util.concurrent.Executors; +import java.util.function.Consumer; + +/** + * A class that represents a subsystem that has motors (rather than something like LEDs). + * This class will automatically stop all the motors when the robot is disabled, and set the motors to brake when the robot is enabled. + * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), then it should override the {@link #setBrake(boolean)} method and do nothing. + */ +public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase { + public static boolean IS_BRAKING = true; + private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); + private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); + private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); + private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); + + static { + DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); + DISABLED_TRIGGER.onFalse(new InstantCommand(() -> { + setAllSubsystemsBrakeAsync(true); + IS_BRAKING = true; + }).ignoringDisable(true)); + } + + private final SysIdRoutine sysIDRoutine = createSysIDRoutine(); + + public MotorSubsystem() { + REGISTERED_SUBSYSTEMS.add(this); + } + + /** + * Runs the given consumer on all the subsystem instances. + * + * @param toRun the consumer to run on each registered subsystem + */ + public static void forEach(Consumer toRun) { + REGISTERED_SUBSYSTEMS.forEach(toRun); + } + + /** + * Sets whether the all the subsystems should brake or coast their motors. + * This command will run asynchronously, since the TalonFX's setting of brake/coast is blocking. + * + * @param brake whether the motors should brake or coast + */ + public static void setAllSubsystemsBrakeAsync(boolean brake) { + BRAKE_MODE_EXECUTOR.execute(() -> forEach((subsystem) -> subsystem.setBrake(brake))); + } + + public static boolean isExtensiveLoggingEnabled() { + return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); + } + + /** + * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one). + * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled. + * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead. + */ + @Override + public final void periodic() { + updatePeriodically(); + if (isExtensiveLoggingEnabled()) + updateMechanism(); + } + + /** + * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism. + * + * @param direction the direction in which to run the test + * @return the command + * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null + */ + public final Command getQuasistaticCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException { + if (sysIDRoutine == null) + throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!"); + return sysIDRoutine.quasistatic(direction); + } + + /** + * Creates a dynamic (constant "step up") command for characterizing the subsystem's mechanism. + * + * @param direction the direction in which to run the test + * @return the command + * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null + */ + public final Command getDynamicCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException { + if (sysIDRoutine == null) + throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!"); + return sysIDRoutine.dynamic(direction); + } + + /** + * Drives the motor with the given voltage for characterizing. + * + * @param targetDrivePower the target drive power, unitless. This can be amps, volts, etc. Depending on the characterization type + */ + public void sysIDDrive(double targetDrivePower) { + } + + /** + * Updates the SysId log of the motor states for characterizing. + * + * @param log the log to update + */ + public void updateLog(SysIdRoutineLog log) { + } + + public SysIdRoutine.Config getSysIDConfig() { + return null; + } + + /** + * Sets whether the subsystem's motors should brake or coast. + * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), don't implement this method. + * + * @param brake whether the motors should brake or coast + */ + public void setBrake(boolean brake) { + } + + /** + * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here. + */ + public void updatePeriodically() { + } + + /** + * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true. + * This doesn't always run in order to save resources. + */ + public void updateMechanism() { + } + + public void changeDefaultCommand(Command newDefaultCommand) { + final Command currentDefaultCommand = getDefaultCommand(); + if (currentDefaultCommand != null) + currentDefaultCommand.cancel(); + setDefaultCommand(newDefaultCommand); + } + + public abstract void stop(); + + private SysIdRoutine createSysIDRoutine() { + if (getSysIDConfig() == null) + return null; + + return new SysIdRoutine( + getSysIDConfig(), + new SysIdRoutine.Mechanism( + (voltage) -> sysIDDrive(voltage.in(Units.Volts)), + this::updateLog, + this, + getName() + ) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java new file mode 100644 index 00000000..3b1f8d52 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -0,0 +1,396 @@ +package frc.trigon.robot.subsystems.swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.phoenix6.Phoenix6SignalThread; +import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; +import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Signal; +import frc.trigon.lib.utilities.flippable.Flippable; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.lib.utilities.flippable.FlippableRotation2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; +import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Swerve extends MotorSubsystem { + private final Pigeon2Gyro gyro = SwerveConstants.GYRO; + private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES; + private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance(); + public Pose2d targetPathPlannerPose = new Pose2d(); + public boolean isPathPlannerDriving = false; + + public Swerve() { + setName("Swerve"); + phoenix6SignalThread.setThreadFrequencyHertz(PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + } + + @Override + public void setBrake(boolean brake) { + for (SwerveModule module : swerveModules) + module.setBrake(brake); + } + + @Override + public void sysIDDrive(double targetVoltage) { + SwerveModuleState[] rotationStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1)); + for (int i = 0; i < 4; i++) { + swerveModules[i].setTargetDriveVoltage(targetVoltage); + swerveModules[i].setTargetSteerAngle(rotationStates[i].angle); + } + } + + @Override + public void updateLog(SysIdRoutineLog log) { + for (SwerveModule module : swerveModules) + module.updateSysIDLog(log); + } + + @Override + public void updatePeriodically() { + Phoenix6SignalThread.SIGNALS_LOCK.lock(); + updateHardware(); + Phoenix6SignalThread.SIGNALS_LOCK.unlock(); + + updatePoseEstimatorStates(); + RobotContainer.ROBOT_POSE_ESTIMATOR.periodic(); + } + + @Override + public SysIdRoutine.Config getSysIDConfig() { + return SwerveModuleConstants.DRIVE_MOTOR_SYSID_CONFIG; + } + + @Override + public void stop() { + for (SwerveModule module : swerveModules) + module.stop(); + } + + public void setHeading(Rotation2d heading) { + gyro.setYaw(heading); + } + + public Rotation2d getHeading() { + return Rotation2d.fromDegrees(SwerveConstants.GYRO.getSignal(Pigeon2Signal.YAW)); + } + + public Translation2d getFieldRelativeVelocity() { + final ChassisSpeeds chassisSpeeds = getFieldRelativeChassisSpeeds(); + return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + } + + public ChassisSpeeds getFieldRelativeChassisSpeeds() { + final ChassisSpeeds selfRelativeSpeeds = getSelfRelativeChassisSpeeds(); + return ChassisSpeeds.fromRobotRelativeSpeeds(selfRelativeSpeeds, RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); + } + + public Translation2d getSelfRelativeVelocity() { + final ChassisSpeeds chassisSpeeds = getSelfRelativeChassisSpeeds(); + return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + } + + public ChassisSpeeds getSelfRelativeChassisSpeeds() { + return SwerveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); + } + + public double getRotationalVelocityRadiansPerSecond() { + return getSelfRelativeChassisSpeeds().omegaRadiansPerSecond; + } + + /** + * Checks if the robot is at a specific pose. + * + * @param flippablePose2d the flippable pose to check + * @return whether the robot is at the pose + */ + public boolean atPose(FlippablePose2d flippablePose2d) { + final Pose2d flippedPose = flippablePose2d.get(); + return atXAxisPosition(flippedPose.getX()) && atYAxisPosition(flippedPose.getY()) && atAngle(flippablePose2d.getRotation()); + } + + public boolean atXAxisPosition(double xAxisPosition) { + final double currentXAxisVelocity = getFieldRelativeChassisSpeeds().vxMetersPerSecond; + return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), xAxisPosition, currentXAxisVelocity); + } + + public boolean atYAxisPosition(double yAxisPosition) { + final double currentYAxisVelocity = getFieldRelativeChassisSpeeds().vyMetersPerSecond; + return atTranslationPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), yAxisPosition, currentYAxisVelocity); + } + + public boolean atAngle(FlippableRotation2d angle) { + final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; + final boolean isAngleStill = Math.abs(getSelfRelativeChassisSpeeds().omegaRadiansPerSecond) < SwerveConstants.ROTATION_VELOCITY_TOLERANCE; + Logger.recordOutput("Swerve/AtTargetAngle/atTargetAngle", atTargetAngle); + Logger.recordOutput("Swerve/AtTargetAngle/isStill", isAngleStill); + return atTargetAngle; + } + + /** + * Gets the positions of the drive wheels in radians. We don't use a {@link Rotation2d} because this method returns distance, not rotations. + * This is used for calculating the diameters of the wheels by finding out how far they moved from the radians turned. + * + * @return the positions of the drive wheels in radians + */ + public double[] getDriveWheelPositionsRadians() { + final double[] swerveModulesPositions = new double[swerveModules.length]; + for (int i = 0; i < swerveModules.length; i++) + swerveModulesPositions[i] = swerveModules[i].getDriveWheelPositionRadians(); + return swerveModulesPositions; + } + + public void setTargetPathPlannerPose(Pose2d targetPathPlannerPose) { + this.targetPathPlannerPose = targetPathPlannerPose; + } + + public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, boolean isFromPathPlanner) { + isPathPlannerDriving = !isStill(targetPathPlannerFeedforwardSpeeds); + if (!isPathPlannerDriving) { + pidToPose(new FlippablePose2d(targetPathPlannerPose, false)); + return; + } + + if (isFromPathPlanner && DriverStation.isAutonomous() && !isPathPlannerDriving) + return; + final ChassisSpeeds pidSpeeds = calculateSelfRelativePIDSpeedsToPose(new FlippablePose2d(targetPathPlannerPose, false)); + final ChassisSpeeds scaledSpeeds = targetPathPlannerFeedforwardSpeeds.times(AutonomousConstants.FEEDFORWARD_SCALAR); + final ChassisSpeeds combinedSpeeds = pidSpeeds.plus(scaledSpeeds); + selfRelativeDrive(combinedSpeeds); + } + + /** + * Calculates and sets the target states for each module from robot-relative chassis speeds. + * + * @param targetSpeeds the desired robot-relative targetSpeeds + */ + public void selfRelativeDrive(ChassisSpeeds targetSpeeds) { +// if (isStill(targetSpeeds) { +// stop(); +// return; +// } + + final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds); + setTargetModuleStates(swerveModuleStates); + } + + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + return Flippable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.k180deg) : currentAngle; + } + + public void initializeDrive(boolean shouldUseClosedLoop) { + setDriveControlMode(shouldUseClosedLoop); + resetRotationController(); + } + + void resetRotationController() { + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees()); + } + + /** + * Moves the robot to a certain pose using PID. + * + * @param targetPose the target pose, relative to the blue alliance driver station's right corner + */ + void pidToPose(FlippablePose2d targetPose) { + final ChassisSpeeds targetSpeeds = calculateSelfRelativePIDSpeedsToPose(targetPose); + selfRelativeDrive(targetSpeeds); + } + + /** + * Drives the swerve with the given powers and a target angle, relative to the field's frame of reference. + * + * @param xPower the x power + * @param yPower the y power + * @param targetAngle the target angle, relative to the blue alliance's forward position + */ + void fieldRelativeDrive(double xPower, double yPower, FlippableRotation2d targetAngle) { + final ChassisSpeeds speeds = selfRelativeSpeedsFromFieldRelativePowers(xPower, yPower, 0); + speeds.omegaRadiansPerSecond = calculateProfiledAngularVelocityRadiansPerSecond(targetAngle); + selfRelativeDrive(speeds); + } + + /** + * Drives the swerve with the given powers, relative to the field's frame of reference. + * + * @param xPower the x power + * @param yPower the y power + * @param thetaPower the theta power + */ + void fieldRelativeDrive(double xPower, double yPower, double thetaPower) { + final ChassisSpeeds speeds = selfRelativeSpeedsFromFieldRelativePowers(xPower, yPower, thetaPower); + selfRelativeDrive(speeds); + } + + /** + * Drives the swerve with the given powers, relative to the robot's frame of reference. + * + * @param xPower the x power + * @param yPower the y power + * @param thetaPower the theta power + */ + void selfRelativeDrive(double xPower, double yPower, double thetaPower) { + final ChassisSpeeds speeds = powersToSpeeds(xPower, yPower, thetaPower); + selfRelativeDrive(speeds); + } + + /** + * Drives the swerve with the given powers and a target angle, relative to the robot's frame of reference. + * + * @param xPower the x power + * @param yPower the y power + * @param targetAngle the target angle + */ + void selfRelativeDrive(double xPower, double yPower, FlippableRotation2d targetAngle) { + final ChassisSpeeds speeds = powersToSpeeds(xPower, yPower, 0); + speeds.omegaRadiansPerSecond = calculateProfiledAngularVelocityRadiansPerSecond(targetAngle); + selfRelativeDrive(speeds); + } + + private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) { + for (int i = 0; i < swerveModules.length; i++) + swerveModules[i].setTargetState(swerveModuleStates[i]); + } + + private void setDriveControlMode(boolean shouldUseClosedLoop) { + for (SwerveModule currentModule : swerveModules) + currentModule.shouldDriveMotorUseClosedLoop(shouldUseClosedLoop); + } + + /** + * Returns whether the given chassis speeds are considered to be "still" by the swerve neutral deadband. + * + * @param chassisSpeeds the chassis speeds to check + * @return true if the chassis speeds are considered to be "still" + */ + private boolean isStill(ChassisSpeeds chassisSpeeds) { + return Math.abs(chassisSpeeds.vxMetersPerSecond) <= SwerveConstants.DRIVE_NEUTRAL_DEADBAND && + Math.abs(chassisSpeeds.vyMetersPerSecond) <= SwerveConstants.DRIVE_NEUTRAL_DEADBAND && + Math.abs(chassisSpeeds.omegaRadiansPerSecond) <= SwerveConstants.ROTATION_NEUTRAL_DEADBAND; + } + + private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { + return new ChassisSpeeds( + xPower * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND, + yPower * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND, + thetaPower * SwerveConstants.MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND + ); + } + + private ChassisSpeeds calculateSelfRelativePIDSpeedsToPose(FlippablePose2d targetPose) { + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(SwerveConstants.PID_TO_POSE_PREDICTION_TIME_SECONDS); + final Pose2d flippedTargetPose = targetPose.get(); + + final double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.atSetpoint() ? + 0 : + SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); + final double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.atSetpoint() ? + 0 : + SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); + + final int directionSign = Flippable.isRedAlliance() ? -1 : 1; + final ChassisSpeeds targetFieldRelativeSpeeds = new ChassisSpeeds( + xSpeed * directionSign, + ySpeed * directionSign, + calculateProfiledAngularVelocityRadiansPerSecond(targetPose.getRotation()) + ); + + return fieldRelativeSpeedsToSelfRelativeSpeeds(targetFieldRelativeSpeeds); + } + + private void updatePoseEstimatorStates() { + final double[] odometryUpdatesYawDegrees = gyro.getThreadedSignal(Pigeon2Signal.YAW); + final int totalOdometryUpdates = odometryUpdatesYawDegrees.length; + final SwerveModulePosition[][] swerveWheelPositions = new SwerveModulePosition[totalOdometryUpdates][]; + final Rotation2d[] gyroRotations = new Rotation2d[totalOdometryUpdates]; + + for (int i = 0; i < totalOdometryUpdates; i++) { + swerveWheelPositions[i] = getSwerveWheelPositions(i); + gyroRotations[i] = Rotation2d.fromDegrees(odometryUpdatesYawDegrees[i]); + } + + RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry(swerveWheelPositions, gyroRotations, phoenix6SignalThread.getLatestTimestamps()); + } + + private SwerveModulePosition[] getSwerveWheelPositions(int odometryUpdateIndex) { + final SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; + for (int i = 0; i < swerveModules.length; i++) + swerveModulePositions[i] = swerveModules[i].getOdometryPosition(odometryUpdateIndex); + return swerveModulePositions; + } + + private boolean atTranslationPosition(double currentTranslationPosition, double targetTranslationPosition, double currentTranslationVelocity) { + return Math.abs(currentTranslationPosition - targetTranslationPosition) < SwerveConstants.TRANSLATION_TOLERANCE_METERS && + Math.abs(currentTranslationVelocity) < SwerveConstants.TRANSLATION_VELOCITY_TOLERANCE; + } + + private double calculateProfiledAngularVelocityRadiansPerSecond(FlippableRotation2d targetAngle) { + if (targetAngle == null) + return 0; + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle.get().getDegrees()); + + final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + final double outputSpeedRadians = Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees())); + final boolean atGoal = SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.atGoal(); + Logger.recordOutput("Swerve/ProfiledRotationPIDController/Setpoint", SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.getSetpoint().position); + Logger.recordOutput("Swerve/ProfiledRotationPIDController/CurrentAngle", currentAngle.getDegrees()); + Logger.recordOutput("Swerve/ProfiledRotationPIDController/AtGoal", atGoal); + Logger.recordOutput("Swerve/ProfiledRotationPIDController/OutputSpeedRadians", outputSpeedRadians); + if (atGoal) + return 0; + + return outputSpeedRadians; + } + + private ChassisSpeeds selfRelativeSpeedsFromFieldRelativePowers(double xPower, double yPower, double thetaPower) { + final ChassisSpeeds fieldRelativeSpeeds = powersToSpeeds(xPower, yPower, thetaPower); + return fieldRelativeSpeedsToSelfRelativeSpeeds(fieldRelativeSpeeds); + } + + private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fieldRelativeSpeeds) { + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); + } + + private void updateHardware() { + gyro.update(); + + for (SwerveModule currentModule : swerveModules) + currentModule.updatePeriodically(); + + phoenix6SignalThread.updateLatestTimestamps(); + } + + @AutoLogOutput(key = "Swerve/CurrentStates") + private SwerveModuleState[] getModuleStates() { + final SwerveModuleState[] states = new SwerveModuleState[swerveModules.length]; + + for (int i = 0; i < swerveModules.length; i++) + states[i] = swerveModules[i].getCurrentState(); + + return states; + } + + @AutoLogOutput(key = "Swerve/TargetStates") + @SuppressWarnings("unused") + private SwerveModuleState[] getTargetStates() { + final SwerveModuleState[] states = new SwerveModuleState[swerveModules.length]; + + for (int i = 0; i < swerveModules.length; i++) + states[i] = swerveModules[i].getTargetState(); + + return states; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java new file mode 100644 index 00000000..243d5e3f --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -0,0 +1,163 @@ +package frc.trigon.robot.subsystems.swerve; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.RobotContainer; +import frc.trigon.lib.commands.InitExecuteCommand; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.lib.utilities.flippable.FlippableRotation2d; + +import java.util.Set; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SwerveCommands { + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param thetaSupplier the target theta power, CCW+ + * @return the command + */ + public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. + * This command will use pid to reach the target angle. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param angleSupplier the target angle supplier + * @return the command + */ + public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier angleSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), angleSupplier.get()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param thetaSupplier the target theta power, CCW+ + * @return the command + */ + public static Command getOpenLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(false), + () -> RobotContainer.SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in open loop mode. + * This command will use pid to reach the target angle. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param angleSupplier the target angle supplier + * @return the command + */ + public static Command getOpenLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier angleSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(false), + () -> RobotContainer.SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), angleSupplier.get()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the robot's frame of reference, in closed loop mode. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param thetaSupplier the target theta power, CCW+ + * @return the command + */ + public static Command getClosedLoopSelfRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.selfRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the robot's frame of reference, in closed loop mode. + * This command will use pid to reach the target angle. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param angleSupplier the target angle supplier + * @return the command + */ + public static Command getClosedLoopSelfRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier angleSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.selfRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), angleSupplier.get()), + RobotContainer.SWERVE + ); + } + + /** + * Creates a command that drives the swerve with the given powers, relative to the robot's frame of reference, in open loop mode. + * + * @param xSupplier the target forwards power + * @param ySupplier the target leftwards power + * @param thetaSupplier the target theta power, CCW+ + * @return the command + */ + public static Command getOpenLoopSelfRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(false), + () -> RobotContainer.SWERVE.selfRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + + public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints) { + return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints), Set.of(RobotContainer.SWERVE)); + } + + public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints, double endVelocity) { + return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints, endVelocity), Set.of(RobotContainer.SWERVE)); + } + + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints) { + return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), + AutoBuilder.pathfindToPose(targetPose.get(), constraints), + getPIDToPoseCommand(targetPose) + ); + } + + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints, double endVelocity) { + return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), + AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity) + ); + } + + private static Command getPIDToPoseCommand(FlippablePose2d targetPose) { + return new FunctionalCommand( + RobotContainer.SWERVE::resetRotationController, + () -> RobotContainer.SWERVE.pidToPose(targetPose), + (interrupted) -> { + }, + () -> RobotContainer.SWERVE.atPose(targetPose) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java new file mode 100644 index 00000000..2f45d2b1 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -0,0 +1,122 @@ +package frc.trigon.robot.subsystems.swerve; + +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.pathplanner.lib.config.PIDConstants; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; +import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Signal; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; + +public class SwerveConstants { + private static final int GYRO_ID = 0; + private static final String GYRO_NAME = "SwerveGyro"; + static final Pigeon2Gyro GYRO = new Pigeon2Gyro(GYRO_ID, GYRO_NAME, RobotConstants.CANIVORE_NAME); + + public static final int + FRONT_LEFT_ID = 1, + FRONT_RIGHT_ID = 2, + REAR_LEFT_ID = 3, + REAR_RIGHT_ID = 4; + private static final double + FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0, + FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0, + REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0, + REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0; + private static final double//TODO:Calibrate + FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2, + FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2, + REAR_LEFT_WHEEL_DIAMETER = 0.05 * 2, + REAR_RIGHT_WHEEL_DIAMETER = 0.05 * 2; + static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{ + new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS, FRONT_LEFT_WHEEL_DIAMETER), + new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS, FRONT_RIGHT_WHEEL_DIAMETER), + new SwerveModule(REAR_LEFT_ID, REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS, REAR_LEFT_WHEEL_DIAMETER), + new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS, REAR_RIGHT_WHEEL_DIAMETER) + }; + + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(AutonomousConstants.ROBOT_CONFIG.moduleLocations); + static final double + TRANSLATION_TOLERANCE_METERS = 0.035, + ROTATION_TOLERANCE_DEGREES = 1.5, + TRANSLATION_VELOCITY_TOLERANCE = 0.15, + ROTATION_VELOCITY_TOLERANCE = 0.3; + static final double + DRIVE_NEUTRAL_DEADBAND = 0.2, + ROTATION_NEUTRAL_DEADBAND = 0.2; + + public static final double + MAXIMUM_SPEED_METERS_PER_SECOND = AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS, + MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND = AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS / AutonomousConstants.ROBOT_CONFIG.modulePivotDistance[0]; + + private static final PIDConstants + TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(5, 0, 0) : + new PIDConstants(6.3, 0, 0), + PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(4, 0, 0) : + new PIDConstants(10, 0, 0.1); + private static final double + MAXIMUM_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : Units.radiansToDegrees(MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND), + MAXIMUM_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 900; + private static final TrapezoidProfile.Constraints ROTATION_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAXIMUM_ROTATION_VELOCITY, + MAXIMUM_ROTATION_ACCELERATION + ); + static final double MAXIMUM_PID_ANGLE = 180; + static final ProfiledPIDController PROFILED_ROTATION_PID_CONTROLLER = new ProfiledPIDController( + PROFILED_ROTATION_PID_CONSTANTS.kP, + PROFILED_ROTATION_PID_CONSTANTS.kI, + PROFILED_ROTATION_PID_CONSTANTS.kD, + ROTATION_CONSTRAINTS + ); + static final PIDController + X_TRANSLATION_PID_CONTROLLER = new PIDController( + TRANSLATION_PID_CONSTANTS.kP, + TRANSLATION_PID_CONSTANTS.kI, + TRANSLATION_PID_CONSTANTS.kD + ), + Y_TRANSLATION_PID_CONTROLLER = new PIDController( + TRANSLATION_PID_CONSTANTS.kP, + TRANSLATION_PID_CONSTANTS.kI, + TRANSLATION_PID_CONSTANTS.kD + ); + private static final double + ROTATION_PID_TOLERANCE_DEGREES = 1, + TRANSLATION_PID_TOLERANCE_METERS = 0.02; + static final double PID_TO_POSE_PREDICTION_TIME_SECONDS = 0.13;//TODO:Calibrate + + static { + configureGyro(); + configurePIDControllers(); + } + + private static void configureGyro() { + final Pigeon2Configuration config = new Pigeon2Configuration(); + //TODO:Calibrate + config.MountPose.MountPoseYaw = 0; + config.MountPose.MountPosePitch = 0; + config.MountPose.MountPoseRoll = 0; + + GYRO.applyConfiguration(config); + GYRO.setSimulationYawVelocitySupplier(() -> Units.radiansToDegrees(RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond())); + + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + } + + private static void configurePIDControllers() { + SwerveConstants.X_TRANSLATION_PID_CONTROLLER.setTolerance(TRANSLATION_PID_TOLERANCE_METERS); + SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.setTolerance(TRANSLATION_PID_TOLERANCE_METERS); + + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setTolerance(ROTATION_PID_TOLERANCE_DEGREES); + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.enableContinuousInput(-SwerveConstants.MAXIMUM_PID_ANGLE, SwerveConstants.MAXIMUM_PID_ANGLE); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java new file mode 100644 index 00000000..e6d83a7d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -0,0 +1,224 @@ +package frc.trigon.robot.subsystems.swerve.swervemodule; + +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; +import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; +import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.utilities.Conversions; + +public class SwerveModule { + private final TalonFXMotor + driveMotor, + steerMotor; + private final CANcoderEncoder steerEncoder; + private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); + private final VelocityVoltage driveVelocityRequest = new VelocityVoltage(0).withUpdateFreqHz(SwerveModuleConstants.DRIVE_VELOCITY_REQUEST_UPDATE_FREQUENCY_HERTZ).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); + private final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); + private final double wheelDiameter; + private boolean shouldDriveMotorUseClosedLoop = true; + private SwerveModuleState targetState = new SwerveModuleState(); + private double[] + latestOdometryDrivePositions, + latestOdometrySteerPositions; + + /** + * Constructs a new SwerveModule with the given module ID, wheel diameter, and encoder offset. + * + * @param moduleID the ID of the module + * @param offsetRotations the module's encoder offset in rotations + * @param wheelDiameter the diameter of the wheel + */ + public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter) { + driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); + steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); + steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + this.wheelDiameter = wheelDiameter; + + configureHardware(offsetRotations); + } + + public void setTargetState(SwerveModuleState targetState) { + if (willOptimize(targetState)) { + targetState.optimize(getCurrentSteerAngle()); + } + + this.targetState = targetState; + setTargetSteerAngle(targetState.angle); + setTargetDriveVelocity(targetState.speedMetersPerSecond); + } + + public void setBrake(boolean brake) { + driveMotor.setBrake(brake); + steerMotor.setBrake(brake); + } + + public void updateSysIDLog(SysIdRoutineLog log) { + log.motor("Module" + driveMotor.getID() + "Drive") + .angularPosition(Units.Rotations.of(driveMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(driveMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(driveMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + } + + /** + * Updates the swerve module. Should be called periodically. + * This method updates the hardware, and their position variables. + * We save their positions to a variable instead of getting them directly because their signals update at a higher frequency than the main code loop. + */ + public void updatePeriodically() { + driveMotor.update(); + steerMotor.update(); + steerEncoder.update(); + + latestOdometryDrivePositions = driveMotor.getThreadedSignal(TalonFXSignal.POSITION); + latestOdometrySteerPositions = steerMotor.getThreadedSignal(TalonFXSignal.POSITION); + } + + public void stop() { + driveMotor.stopMotor(); + steerMotor.stopMotor(); + } + + public void shouldDriveMotorUseClosedLoop(boolean shouldDriveMotorUseClosedLoop) { + this.shouldDriveMotorUseClosedLoop = shouldDriveMotorUseClosedLoop; + } + + public void setTargetDriveVoltage(double targetVoltage) { + driveMotor.setControl(driveVoltageRequest.withOutput(targetVoltage)); + } + + public void setTargetSteerAngle(Rotation2d angle) { + steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); + } + + public SwerveModuleState getCurrentState() { + return new SwerveModuleState(driveWheelRotationsToMeters(driveMotor.getSignal(TalonFXSignal.VELOCITY)), getCurrentSteerAngle()); + } + + public SwerveModuleState getTargetState() { + return targetState; + } + + /** + * Gets the position of the drive wheel in meters. We don't use a {@link Rotation2d} because this function returns distance, not rotations. + * + * @return the position of the drive wheel in meters + */ + public double getDriveWheelPositionRadians() { + return edu.wpi.first.math.util.Units.rotationsToRadians(driveMotor.getSignal(TalonFXSignal.POSITION)); + } + + /** + * Since the odometry updates faster than the main code thread, + * the odometry position is calculated with all odometry positions since the last main thread update. + * + * @param odometryUpdateIndex the index of the new odometry information since the last main thread update + * @return the position of the module at the given odometry update index + */ + public SwerveModulePosition getOdometryPosition(int odometryUpdateIndex) { + return new SwerveModulePosition( + driveWheelRotationsToMeters(latestOdometryDrivePositions[odometryUpdateIndex]), + Rotation2d.fromRotations(latestOdometrySteerPositions[odometryUpdateIndex]) + ); + } + + private boolean willOptimize(SwerveModuleState state) { + final Rotation2d angularDelta = state.angle.minus(getCurrentSteerAngle()); + return Math.abs(angularDelta.getRadians()) > Math.PI / 2; + } + + /** + * Sets the target drive velocity for the module. + * The target velocity is set using either closed loop or open loop control, depending on {@link this#shouldDriveMotorUseClosedLoop}. + * + * @param targetVelocityMetersPerSecond the target drive velocity, in meters per second + */ + private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) { + if (shouldDriveMotorUseClosedLoop) { + setTargetClosedLoopDriveVelocity(targetVelocityMetersPerSecond); + return; + } + + setTargetOpenLoopDriveVelocity(targetVelocityMetersPerSecond); + } + + private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) { + final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond); + + driveMotor.setControl(driveVelocityRequest.withVelocity(targetDriveVelocityRotationsPerSecond)); + } + + private void setTargetOpenLoopDriveVelocity(double targetVelocityMetersPerSecond) { + final double targetDrivePower = targetVelocityMetersPerSecond / SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND; + final double targetDriveVoltage = Conversions.compensatedPowerToVoltage(targetDrivePower, SwerveModuleConstants.VOLTAGE_COMPENSATION_SATURATION); + driveMotor.setControl(driveVoltageRequest.withOutput(targetDriveVoltage)); + } + + private Rotation2d getCurrentSteerAngle() { + return Rotation2d.fromRotations(steerMotor.getSignal(TalonFXSignal.POSITION)); + } + + /** + * Converts drive wheel rotations distance to meters. + * + * @param rotations the rotations of the drive wheel + * @return the distance the drive wheel has traveled in meters + */ + private double driveWheelRotationsToMeters(double rotations) { + return Conversions.rotationsToDistance(rotations, wheelDiameter); + } + + /** + * Converts meters to number of rotations from the drive wheel. + * + * @param meters the meters to be converted + * @return the distance the drive wheel has traveled in drive wheel rotations + */ + private double metersToDriveWheelRotations(double meters) { + return Conversions.distanceToRotations(meters, wheelDiameter); + } + + private void configureHardware(double offsetRotations) { + configureDriveMotor(); + configureSteerMotor(); + configureSteerEncoder(offsetRotations); + } + + private void configureDriveMotor() { + driveMotor.applyConfiguration(SwerveModuleConstants.generateDriveMotorConfiguration()); + driveMotor.setPhysicsSimulation(SwerveModuleConstants.createDriveMotorSimulation()); + + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); + driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); + driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + } + + private void configureSteerMotor() { + steerMotor.applyConfiguration(SwerveModuleConstants.generateSteerMotorConfiguration(steerMotor.getID())); + steerMotor.setPhysicsSimulation(SwerveModuleConstants.createSteerMotorSimulation()); + + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); + steerMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); + steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + } + + private void configureSteerEncoder(double offsetRotations) { + steerEncoder.applyConfiguration(SwerveModuleConstants.generateSteerEncoderConfiguration(offsetRotations)); + steerEncoder.setSimulationInputsFromTalonFX(steerMotor); + + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); + steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java new file mode 100644 index 00000000..e2f76fc4 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -0,0 +1,126 @@ +package frc.trigon.robot.subsystems.swerve.swervemodule; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.hardware.simulation.SimpleMotorSimulation; +import frc.trigon.robot.constants.AutonomousConstants; + +public class SwerveModuleConstants { + private static final double + DRIVE_MOTOR_GEAR_RATIO = 7.13, + REAR_STEER_MOTOR_GEAR_RATIO = 12.8; + static final boolean ENABLE_FOC = true; + + private static final double + DRIVE_MOMENT_OF_INERTIA = 0.003, + STEER_MOMENT_OF_INERTIA = 0.003; + private static final int + DRIVE_MOTOR_AMOUNT = 1, + STEER_MOTOR_AMOUNT = 1; + private static final DCMotor + DRIVE_MOTOR_GEARBOX = DCMotor.getKrakenX60Foc(DRIVE_MOTOR_AMOUNT), + STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); + + public static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(1).per(Units.Second), + Units.Volts.of(2), + Units.Second.of(1000) + ); + + public static final double MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND = edu.wpi.first.math.util.Units.rotationsToRadians(7); //TODO: calibrate + static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static final double DRIVE_VELOCITY_REQUEST_UPDATE_FREQUENCY_HERTZ = 1000; + + /** + * Creates a new SimpleMotorSimulation for the drive motor. + * We use a function instead of a constant because we need to create a new instance of the simulation for each module. + * + * @return the drive motor simulation + */ + static SimpleMotorSimulation createDriveMotorSimulation() { + return new SimpleMotorSimulation(DRIVE_MOTOR_GEARBOX, DRIVE_MOTOR_GEAR_RATIO, DRIVE_MOMENT_OF_INERTIA); + } + + /** + * Creates a new SimpleMotorSimulation for the steer motor. + * We use a function instead of a constant because we need to create a new instance of the simulation for each module. + * + * @return the steer motor simulation + */ + static SimpleMotorSimulation createSteerMotorSimulation() { + return new SimpleMotorSimulation(STEER_MOTOR_GEARBOX, REAR_STEER_MOTOR_GEAR_RATIO, STEER_MOMENT_OF_INERTIA); + } + + static TalonFXConfiguration generateDriveMotorConfiguration() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.Feedback.SensorToMechanismRatio = DRIVE_MOTOR_GEAR_RATIO; + + final double driveMotorSlipCurrent = AutonomousConstants.ROBOT_CONFIG.moduleConfig.driveCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = driveMotorSlipCurrent; + config.TorqueCurrent.PeakReverseTorqueCurrent = -driveMotorSlipCurrent; + config.CurrentLimits.StatorCurrentLimit = driveMotorSlipCurrent; + config.CurrentLimits.StatorCurrentLimitEnable = true; + + config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 00.020691 : 0; + + config.Feedback.VelocityFilterTimeConstant = 0; + + return config; + } + + static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSensorID) { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = true; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 50; + config.CurrentLimits.StatorCurrentLimitEnable = true; + + config.Feedback.RotorToSensorRatio = REAR_STEER_MOTOR_GEAR_RATIO; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + config.Feedback.FeedbackRemoteSensorID = feedbackRemoteSensorID; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 40; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.ClosedLoopGeneral.ContinuousWrap = true; + + return config; + } + + static CANcoderConfiguration generateSteerEncoderConfiguration(double offsetRotations) { + final CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = offsetRotations; + + return config; + } +} \ No newline at end of file diff --git a/src/main/python/keyboard_to_nt.py b/src/main/python/keyboard_to_nt.py new file mode 100644 index 00000000..f605d1a5 --- /dev/null +++ b/src/main/python/keyboard_to_nt.py @@ -0,0 +1,33 @@ +import ntcore +import keyboard +import time + +def main(): + def on_action(event: keyboard.KeyboardEvent): + if event.name == "/": + return + if event.is_keypad: + table.putBoolean("numpad"+event.name, event.event_type == keyboard.KEY_DOWN) + else: + table.putBoolean(event.name.lower(), event.event_type == keyboard.KEY_DOWN) + + ntcoreinst = ntcore.NetworkTableInstance.getDefault() + + print("Setting up NetworkTables client") + ntcoreinst.startClient4("KeyboardToNT") + ntcoreinst.setServer("127.0.0.1") + ntcoreinst.startDSClient() + + # Wait for connection + print("Waiting for connection to NetworkTables server...") + while not ntcoreinst.isConnected(): + time.sleep(0.1) + + print("Connected!") + table = ntcoreinst.getTable("SmartDashboard/keyboard") + + keyboard.hook(lambda e: on_action(e)) + keyboard.wait() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/main/python/t265_manager.py b/src/main/python/t265_manager.py new file mode 100644 index 00000000..0192e2fd --- /dev/null +++ b/src/main/python/t265_manager.py @@ -0,0 +1,58 @@ +import pyrealsense2 as rs +import ntcore +import json + +class T265Manager: + def __init__(self, device_id=None): + self.pipeline = rs.pipeline() + config = rs.config() + if device_id: + config.enable_device(device_id) + + config.enable_stream(rs.stream.pose) + self.pipeline.start(config) + self.table = self.start_nt_and_get_table(device_id) + + def start_nt_and_get_table(self, name): + instance = ntcore.NetworkTableInstance.getDefault() + instance.setServer("localhost") + table_name = f"T265" + if name: + table_name += f"/{name}" + instance.startClient4(name) + return instance.getTable(table_name) + + def run(self): + frames = self.pipeline.wait_for_frames() + pose_frame = frames.get_pose_frame() + if not pose_frame: + return + + data = self.format_pose_frame(pose_frame) + self.table.putString("jsonDump", json.dumps(data)) + self.table.putNumber("confidence", data["confidence"]) + + @staticmethod + def format_pose_frame(pose_frame): + data = pose_frame.get_pose_data() + confidence = data.tracker_confidence + return { + "translation": [ + data.translation.x, + data.translation.y, + data.translation.z + ], + "rotation": [ + data.rotation.w, + data.rotation.x, + data.rotation.y, + data.rotation.z + ], + "confidence": confidence + } + + +if __name__ == "__main__": + t265_manager = T265Manager("908412110743") + while True: + t265_manager.run() diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..bef4a151 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json new file mode 100644 index 00000000..d3f84e53 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.7.json", + "name": "PathplannerLib", + "version": "2025.2.7", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.7" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.7", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix5-frc2025-latest.json b/vendordeps/Phoenix5-frc2025-latest.json new file mode 100644 index 00000000..c1098dcc --- /dev/null +++ b/vendordeps/Phoenix5-frc2025-latest.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-frc2025-latest.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 00000000..6f40c840 --- /dev/null +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,479 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.1.json new file mode 100644 index 00000000..904ce74a --- /dev/null +++ b/vendordeps/REVLib-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.3", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..3718e0ac --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 00000000..f672f44d --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.3", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.3", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..2d7b1d8e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +} \ No newline at end of file