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 00000000..9e54508d Binary files /dev/null and b/files/images/githublogo.png differ diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..a4b76b95 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ 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