commit c2ffb70f3d905c357950a9cc9af3db89805cc164 Author: HarryWangOnLaptop Date: Mon Feb 23 08:38:49 2026 +0800 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..34cbaac --- /dev/null +++ b/.gitignore @@ -0,0 +1,187 @@ +# 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 +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /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 0000000..5e6ede8 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,61 @@ +{ + "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.*", + ], + "java.dependency.enableDependencyCheckup": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..4b9201c --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 6494 +} \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..645e542 --- /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 0000000..40494f8 --- /dev/null +++ b/build.gradle @@ -0,0 +1,106 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2026.2.1" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.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')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +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() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// 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' + options.encoding = "UTF-8" +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..a4b76b9 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 0000000..34bd9ce --- /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 0000000..f5feea6 --- /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 0000000..9d21a21 --- /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 0000000..486702a --- /dev/null +++ b/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2026' + 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/example.txt b/src/main/deploy/example.txt new file mode 100644 index 0000000..bb82515 --- /dev/null +++ b/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Line.auto b/src/main/deploy/pathplanner/autos/Line.auto new file mode 100644 index 0000000..9bc829a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Line.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Line" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LineGoAndBack.auto b/src/main/deploy/pathplanner/autos/LineGoAndBack.auto new file mode 100644 index 0000000..986c90a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LineGoAndBack.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Line" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "LineBack" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate.auto b/src/main/deploy/pathplanner/autos/Rotate.auto new file mode 100644 index 0000000..20377f4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Rotate.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Rotate" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RotateAndLine.auto b/src/main/deploy/pathplanner/autos/RotateAndLine.auto new file mode 100644 index 0000000..b696103 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RotateAndLine.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RotateAndLine" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,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,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,true,true,true,true,true,true,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,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,true,true,true,true,true,true,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,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,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,false,false,false,false,false,false,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,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,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,true,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,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,true,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,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,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Line.path b/src/main/deploy/pathplanner/paths/Line.path new file mode 100644 index 0000000..d632b3d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Line.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.314, + "y": 4.15 + }, + "prevControl": null, + "nextControl": { + "x": 14.299415121255347, + "y": 4.61077032810271 + }, + "isLocked": false, + "linkedName": "StartingPointRed1" + }, + { + "anchor": { + "x": 14.314, + "y": 6.668017118402283 + }, + "prevControl": { + "x": 14.247660485021397, + "y": 6.305734664764623 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LineBack.path b/src/main/deploy/pathplanner/paths/LineBack.path new file mode 100644 index 0000000..c642657 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LineBack.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.314, + "y": 6.668017118402283 + }, + "prevControl": null, + "nextControl": { + "x": 14.299415121255347, + "y": 7.128787446504993 + }, + "isLocked": false, + "linkedName": "Point1" + }, + { + "anchor": { + "x": 14.314, + "y": 4.15 + }, + "prevControl": { + "x": 14.299415121255347, + "y": 4.597831669044222 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StartingPointRed2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rotate.path b/src/main/deploy/pathplanner/paths/Rotate.path new file mode 100644 index 0000000..b509010 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rotate.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.314, + "y": 4.15 + }, + "prevControl": null, + "nextControl": { + "x": 14.064, + "y": 4.15 + }, + "isLocked": false, + "linkedName": "StartingPointRed1" + }, + { + "anchor": { + "x": 14.314, + "y": 4.15 + }, + "prevControl": { + "x": 14.650181888846735, + "y": 4.15 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RotateAndLine.path b/src/main/deploy/pathplanner/paths/RotateAndLine.path new file mode 100644 index 0000000..968b692 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RotateAndLine.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.314, + "y": 4.15 + }, + "prevControl": null, + "nextControl": { + "x": 14.312353780313838, + "y": 4.74015691868759 + }, + "isLocked": false, + "linkedName": "StartingPointRed1" + }, + { + "anchor": { + "x": 14.314, + "y": 6.357489300998574 + }, + "prevControl": { + "x": 14.312353780313838, + "y": 6.098716119828817 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": 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 0000000..f80d618 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,34 @@ +{ + "robotWidth": 0.85, + "robotLength": 0.85, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 1.5, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 30.0, + "robotMOI": 3.6, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.045, + "driveGearing": 5.902, + "maxDriveSpeed": 4.89, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 40.0, + "wheelCOF": 1.2, + "flModuleX": 0.289, + "flModuleY": 0.289, + "frModuleX": 0.289, + "frModuleY": -0.289, + "blModuleX": -0.289, + "blModuleY": 0.289, + "brModuleX": -0.289, + "brModuleY": -0.289, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"radius\":0.1,\"strokeWidth\":0.02,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..94fda50 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,98 @@ +package frc.robot; + +import com.ctre.phoenix6.configs.Slot0Configs; +import edu.wpi.first.math.geometry.Pose2d; + +public final class Constants { + public static final boolean LEDUsing = false; + public class Shooter { + public static final double conveyorSpeedkP = 10; + public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() + .withKP(0.0).withKI(0).withKD(0) + .withKV(0.1).withKA(0.0).withKS(0.15); + + public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() + .withKP(0.0).withKI(0).withKD(0) + .withKV(0.02).withKA(0.15).withKS(0); + + public static final Slot0Configs backboardSlot0Configs = new Slot0Configs() + .withKP(0.01).withKI(0).withKD(0) + .withKV(0.008).withKA(0.0).withKS(0.010); + + public class backboardPositionPID { + public static final double kP = 0.1; + public static final double kI = 0.0; + public static final double kD = 0.0; + + } + public static final double backboardSpeedMax = 60; //也就是output + // 背板的上下限位角度 + public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置） + public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置） + } + public class ShooterCalculation{ + public static final double positionNear = 0; // 近距离背板角度 + public static final double positionMid = 1600; // 中距离背板角度 + public static final double postionFar = 3200; // 远距离背板角度 + } + public class AutoRotation { + public static final boolean autoRotationToHubEnabled = true; + public static final double autoRotationToHubkP = 0.12; + public static final boolean autoRotationForBump = false; + public static final double autoRotationForBumpkP = 0.2; + public static final double autoRotationForBumpTargetDegrees = 45; + + public static final boolean teleopOffsetEnabled = true; + public static final double teleopOffsetkP = 15; + } + + public class StartingPoints{ + public class Red{ + public static final Pose2d Point1 = new Pose2d(12.93, 7.47, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(0)); + public static final Pose2d Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180)); + } + + } + + public enum DriveMode { + ROBOT_CENTRIC, + FIELD_CENTRIC, + SYSID, + OTHER + } + public class Pigeon { + public static final int CANID = 0; + } + + public class Limelight { + public static final String LIMELIGHT_NAME = "limelight-zy"; + } + + public class Field { + public static final double RedHubPositionX = 11.914; + public static final double RedHubPositionY = 4.034; + + } + + public class Intaker { + + public static final double intakeRotaterUpLimit = 0.0; // rotator上限角度 + public static final double intakeRotaterDownLimit = 0.0; // rotator下限角度 + + public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs() + .withKP(0.00).withKI(0.00).withKD(0.00) + .withKV(0.00).withKA(0.00).withKS(0.00); + // getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI) + + public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs() + .withKP(0.00).withKI(0.00).withKD(0.00) + .withKV(0.00).withKA(0.00).withKS(0.00); + //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI) + + public static final double IntakeGetterSpeed = 0.0; + // 单位RPS + public static final double positionToleranceRotations = 0.00; + //rotater容差 + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..fe215d7 --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,15 @@ +// 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.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/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..9fcb052 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,76 @@ +// 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.robot; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private final RobotContainer m_robotContainer; + + public Robot() { + m_robotContainer = new RobotContainer(); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + m_robotContainer.tunerPeriodic(); + + } + + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + @Override + public void disabledExit() {} + + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + if (m_autonomousCommand != null) { + CommandScheduler.getInstance().schedule(m_autonomousCommand); + } + } + + @Override + public void autonomousPeriodic() {} + + @Override + public void autonomousExit() {} + + @Override + public void teleopInit() { + if (m_autonomousCommand != null) { + CommandScheduler.getInstance().cancel(m_autonomousCommand); + } + } + + @Override + public void teleopPeriodic() {} + + @Override + public void teleopExit() {} + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() {} + + @Override + public void testExit() {} + + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..71eac46 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,134 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import frc.robot.controls.DriveControls; +import frc.robot.controls.ShooterControls; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.tuning.ConfigTalonFXMotorTuner; +import frc.robot.tuning.ConfigTalonFXSMotorTuner; +import frc.robot.tuning.DriveGainsTuner; +import frc.robot.utils.LedBindings; + +public class RobotContainer { + + /* ====================== */ + /* 手柄 */ + /* ====================== */ + + // 只使用一个 Xbox 手柄（端口 0） + private final CommandXboxController controller = new CommandXboxController(0); + + /* ====================== */ + /* 子系统 */ + /* ====================== */ + + // Phoenix TunerX 生成的底盘 + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + + // LED 子系统 + private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72 + + /* ====================== */ + /* 控制封装/日志 */ + /* ====================== */ + + private final DriveControls driveControls = new DriveControls(drivetrain, controller); + private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller); + + private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain); + private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs); + private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs); + private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs); + + private final SendableChooser autoChooser; + + public RobotContainer() { + SignalLogger.enableAutoLogging(false); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + configueSwerve(); + configueShooter(); + bingdingLED(); + } + + /* ====================== */ + /* 底盘配置 */ + /* ====================== */ + + private void configueSwerve() { + Pose2d startingPose = Constants.StartingPoints.Red.Point2; + drivetrain.resetPose(startingPose); + + // 默认驾驶命令 + drivetrain.setDefaultCommand(driveControls.defaultDriveCommand()); + + // Disabled 时进入 idle（防止模块乱动） + RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand()); + + + //左保险：按住刹车 + controller.leftBumper() + .onTrue(Commands.runOnce(driveControls::emergencyStop)) + .whileTrue(driveControls.brakeWhileHeld()); + + // Back：重置场地坐标系 + controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + + // Start：切换场地/车体坐标系 + controller.start().onTrue(Commands.runOnce(driveControls::toggleDriveFrame)); + + // 右保险：按住加速（Boost） + controller.rightBumper() + .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true))) + .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); + + controller.a().whileTrue(drivetrain.runOnce(drivetrain::forceUsingLimelightMT2)); + + } + + private void configueShooter(){ + shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand()); + + controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-5))); + controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(5))); + controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(1500))); + controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-1500))); + + controller.b().onTrue(shooterControls.resetBackboardCommand()); + } + + /* ====================== */ + /* LED 绑定 */ + /* ====================== */ + + private void bingdingLED() { + LedBindings.bindModeIndicators(leds); + } + + public Command getAutonomousCommand() { + + return autoChooser.getSelected(); + } + + public void tunerPeriodic() { + driveGainsTuner.periodic(); + configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight()); + configConveyorTuner.periodic(null); + configBackboardTuner.periodic(); + shooterSubsystem.setRightFollowLeft(); + } + +} diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java new file mode 100644 index 0000000..570fd70 --- /dev/null +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -0,0 +1,243 @@ +package frc.robot.controls; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import frc.robot.Constants.DriveMode; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +public class DriveControls { + private final CommandSwerveDrivetrain drivetrain; + private final CommandXboxController driver; + + private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond); + + // 默认速度比例（平时开车） + private double normalDriveScale = 0.3; + private double normalTurnScale = 0.7; + + // 加速模式比例（按住 RB） + private double boostDriveScale = 1.0; + private double boostTurnScale = 1.0; + + private boolean boostEnabled = false; + + private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效（调） + private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0); + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调） + + // ====== 类成员变量里新增：记录上一次输出 ====== + private double lastVx = 0.0; + private double lastVy = 0.0; + private double lastOmega = 0.0; + + private NetworkTable autoControlNetworkTable; + + public void setBoostEnabled(boolean enabled) { + boostEnabled = enabled; + } + + // 新增：当前是否使用场地坐标系（默认 true） + private boolean fieldCentricEnabled = true; + + private final SwerveRequest.FieldCentric fieldCentric = + new SwerveRequest.FieldCentric() + .withDeadband(maxSpeed * 0.05) + .withRotationalDeadband(maxAngularRate * 0.05) + .withDriveRequestType(DriveRequestType.Velocity); + + private final SwerveRequest.RobotCentric robotCentric = + new SwerveRequest.RobotCentric() + .withDeadband(maxSpeed * 0.05) + .withRotationalDeadband(maxAngularRate * 0.05) + .withDriveRequestType(DriveRequestType.Velocity); + + public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver) { + this.drivetrain = drivetrain; + this.driver = driver; + autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl"); + } + + public void toggleDriveFrame() { + fieldCentricEnabled = !fieldCentricEnabled; + } + + public boolean isFieldCentricEnabled() { + return fieldCentricEnabled; + } + public DriveMode getCurrentDriveMode() { + return fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC; + } + /** 底盘的默认驾驶命令（teleop 期间持续执行） */ + public Command defaultDriveCommand() { + return drivetrain.applyRequest(() -> { + double driveScale = boostEnabled ? boostDriveScale : normalDriveScale; + double turnScale = boostEnabled ? boostTurnScale : normalTurnScale; + + double vx = -driver.getLeftY() * maxSpeed * driveScale; + double vy = -driver.getLeftX() * maxSpeed * driveScale; + double omega = -driver.getRightX() * maxAngularRate * turnScale; + + // ====== execute 内替换 limiter：只限加速、不限减速 ====== + double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter); + double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter); + double limitedOmega = limitAccelOnly(omega, lastOmega, omegaLimiter); + + lastVx = limitedVx; + lastVy = limitedVy; + lastOmega = limitedOmega; + + vx = limitedVx; + vy = limitedVy; + omega = limitedOmega; + + double[] distanceAndRotation = calculateDistanceAndRotationToHub(); + // 将距离值写入 NetworkTable + autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); + autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); + + if(Constants.AutoRotation.autoRotationToHubEnabled) { + double autoRotationPower = driver.getLeftTriggerAxis(); + if(autoRotationPower > 0.1) { + autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(true); + + double angleDifference = distanceAndRotation[1]; + + // Calculate rotation speed using the angle difference + omega = calculateRotationSpeedFromRotationAngle(angleDifference, autoRotationPower); + + autoControlNetworkTable.getEntry("autoRotationRate").setDouble(omega); + } else { + autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(false); + } + } + + + + if (fieldCentricEnabled) { + return fieldCentric + .withVelocityX(vx) + .withVelocityY(vy) + .withRotationalRate(omega); + } else { + return robotCentric + .withVelocityX(vx) + .withVelocityY(vy) + .withRotationalRate(omega); + } + }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); + } + + // Calculate distance and rotation (angle difference) + public double[] calculateDistanceAndRotationToHub() { + Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 + double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标 + double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标 + + // 计算目标角度（相对于场地坐标系） + double deltaX = targetX - currentPose.getX(); + double deltaY = targetY - currentPose.getY(); + double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度 + + // 当前机器人角度 + double currentAngle = currentPose.getRotation().getDegrees(); + double angleDifference = targetAngle - currentAngle; + // Normalize angle difference to the range [-180, 180] + if (angleDifference > 180) { + angleDifference -= 360; + } else if (angleDifference < -180) { + angleDifference += 360; + } + + // 计算距离 + double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY); + // 计算距离和旋转角度 + double[] distanceAndRotation = new double[]{distance, angleDifference}; + + // 返回距离和角度差 + return distanceAndRotation; + } + + // Calculate rotation speed from rotation angle difference + public double calculateRotationSpeedFromRotationAngle(double angleDifference, double autoRotationPower) { + double calDifference = angleDifference; + + // Optional: Apply teleop offset if enabled + if (Constants.AutoRotation.teleopOffsetEnabled) { + double teleopOffset = -driver.getRightX(); + calDifference += teleopOffset * Constants.AutoRotation.teleopOffsetkP; + } + + // If angle difference is within threshold, don't rotate + if (Math.abs(calDifference) > 1.0) { + return calDifference * autoRotationPower * Constants.AutoRotation.autoRotationToHubkP; + } else { + return 0; + } + } + + + + /** + * 只限制“加速”（幅值变大），不限制“减速”（幅值变小） + * 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击 + */ + private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) { + // 如果正在减速（幅值变小），直接放行到目标值 + if (Math.abs(target) < Math.abs(last)) { + limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变 + return target; + } + // 否则是加速，使用 limiter + return limiter.calculate(target); + } + + /** 紧急停止：清零限速器，让速度输出立刻归零 */ + public void emergencyStop() { + vxLimiter.reset(0.0); + vyLimiter.reset(0.0); + omegaLimiter.reset(0.0); + + lastVx = 0.0; + lastVy = 0.0; + lastOmega = 0.0; + } + + public Command idleCommand() { + final var idle = new SwerveRequest.Idle(); + return drivetrain.applyRequest(() -> idle).ignoringDisable(true); + } + + public Command brakeWhileHeld() { + final var brake = new SwerveRequest.SwerveDriveBrake(); + return drivetrain.applyRequest(() -> brake); + } + + + public SwerveRequest.SwerveDriveBrake brakeRequest() { + return new SwerveRequest.SwerveDriveBrake(); + } + + public SwerveRequest.Idle idleRequest() { + return new SwerveRequest.Idle(); + } + + public double getMaxSpeed() { + return maxSpeed; + } + + public double getMaxAngularRate() { + return maxAngularRate; + } + +} diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java new file mode 100644 index 0000000..084b021 --- /dev/null +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -0,0 +1,71 @@ +package frc.robot.controls; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.IntakerSubsystem; + +public class IntakerControls { + private final IntakerSubsystem intakerSubsystem; + private final CommandXboxController controller; + + private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI） + + public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) { + this.intakerSubsystem = intakerSubsystem; + this.controller = controller; + } + + public void configureBindings() { + + //B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转 + controller.a() + .whileTrue( + Commands.run(() -> { + // rotater下降：范围会被 subsystem 限制 + double cur = intakerSubsystem.getIntakerotaterPosition(); + intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE); + + // getter 开 + intakerSubsystem.setIntakeGetterOn(true); + }, intakerSubsystem) + ) + .onFalse( + Commands.runOnce(() -> { + // 松：把当前位置作为target保持 + double cur = intakerSubsystem.getIntakerotaterPosition(); + intakerSubsystem.setIntakerRotaterPosition(cur); + + // gtter继续转 + intakerSubsystem.setIntakeGetterOn(true); + }, intakerSubsystem) + ); + + // === B.B：按住上升+getter停；松开后rotater保持当前位置 + controller.b() + .whileTrue( + Commands.run(() -> { + // rotater升 + double cur = intakerSubsystem.getIntakerotaterPosition(); + intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE); + + // getter关 + intakerSubsystem.setIntakeGetterOn(false); + }, intakerSubsystem) + ) + .onFalse( + Commands.runOnce(() -> { + // 松：保持当前位置 + double cur = intakerSubsystem.getIntakerotaterPosition(); + intakerSubsystem.setIntakerRotaterPosition(cur); + + // getter仍然停止 + intakerSubsystem.setIntakeGetterOn(false); + }, intakerSubsystem) + ); + } + + public Command defaultIntakerCommand() { + return Commands.none(); + } +} diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java new file mode 100644 index 0000000..b850b8f --- /dev/null +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -0,0 +1,143 @@ +package frc.robot.controls; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.Mode; + +public class ShooterControls { + private final ShooterSubsystem shooterSubsystem; + private final CommandXboxController controller; + + private double flywheelSpeedOffset = 0.0; + private double backboardPositionOffset = 0.0; + + public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller) { + this.shooterSubsystem = shooterSubsystem; + this.controller = controller; + } + + public Command defaultShooterCommand() { + return Commands.run(() -> { + // System.out.println("running"); + if (controller.getRightTriggerAxis() < 0.1) { + shooterSubsystem.setConveyorSpeedByRPS(0); + shooterSubsystem.setFlywheelSpeedByRPS(0); + return; + } + double flywheelSpeed; + double backboardPosition; + double conveyorSpeed; + flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(20); + backboardPosition = calculateBackboardPositionOnlywithOffset(100); + conveyorSpeed = controller.getRightTriggerAxis() * Constants.Shooter.conveyorSpeedkP; + + // double flywheelSpeed = calculateFlywheelSpeed(0); + // double BackboardPosition = calculateBackboardPosition(0); + // shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); + // shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed); + shooterSubsystem.setBackboardPosition(backboardPosition); + }, shooterSubsystem); + } + + // public Command resetBackboardCommand(){ + // return + // Commands.runOnce(()->{ + // shooterSubsystem.backboardMotor.set(-0.1); + // shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem) + // .andThen( + // Commands.waitSeconds(0.5)) + // .andThen(Commands.runOnce(()->{ + // shooterSubsystem.backboardMotor.set(0); + // shooterSubsystem.backboardEncoder.reset(); + // shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem)) + // .andThen( + // Commands.waitSeconds(1)) + // .andThen(Commands.runOnce(()->{ + // shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem)); + // } + + public Command resetBackboardCommand(){ + return + Commands.runOnce(()->{ + shooterSubsystem.backboardEncoder.reset(); + shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem); + } + + //打表参数 + private static final double[][] FLYWHEEL_TABLE = { + {1.5, 0.42}, + {2.0, 0.46}, + {2.5, 0.51}, + {3.0, 0.57}, + {3.5, 0.64}, + {4.0, 0.72} + }; + + //线性插值函数 + private static double lerp(double x0, double y0, double x1, double y1, double x) { + if (Math.abs(x1 - x0) < 1e-9) { + return y0; + } + double t = (x - x0) / (x1 - x0); + return y0 + t * (y1 - y0); + } + private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) { + return defaultSpeed + flywheelSpeedOffset; + } + private double calculateBackboardPositionOnlywithOffset(double defaultPosition) { + return defaultPosition + backboardPositionOffset; + } + + private double calculateFlywheelSpeed(double distance) { + // 小于最小距离：用第一个点 + if (distance <= FLYWHEEL_TABLE[0][0]) { + return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset; + } + // 大于最大距离：用最后一个点 + int last = FLYWHEEL_TABLE.length - 1; + if (distance >= FLYWHEEL_TABLE[last][0]) { + return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset; + } + // 查找区间并插值 + for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) { + double d0 = FLYWHEEL_TABLE[i][0]; + double s0 = FLYWHEEL_TABLE[i][1]; + double d1 = FLYWHEEL_TABLE[i + 1][0]; + double s1 = FLYWHEEL_TABLE[i + 1][1]; + if (distance >= d0 && distance <= d1) { + return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset; + } + } + return 0; + } + + private double calculateBackboardPosition(double distance) { + double BackboardPosition; + // 距离阈值（单位自己按distance进行修改） + final double DISTANCE_NEAR = 2.0; // 近距离阈值 + final double DISTANCE_MID = 4.0; // 中距离阈值 + if (distance < DISTANCE_NEAR) { + BackboardPosition = Constants.ShooterCalculation.positionNear; + } + else if (distance < DISTANCE_MID) { + BackboardPosition = Constants.ShooterCalculation.positionMid; + } + else { + BackboardPosition = Constants.ShooterCalculation.postionFar; + } + BackboardPosition += backboardPositionOffset; + return BackboardPosition; + } + + public void adjustFlywheelSpeedOffset(double offset) { + flywheelSpeedOffset += offset; + } + + public void adjustBackboardRateOffset(double offset) { + backboardPositionOffset += offset; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..05b7dfc --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,286 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the 2026 Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(70).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.59).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.01).withKI(0).withKD(0) + .withKS(0.05).withKV(0.117); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.125; + + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(1.81); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 11; + private static final int kFrontLeftSteerMotorId = 12; + private static final int kFrontLeftEncoderId = 13; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(11.375); + private static final Distance kFrontLeftYPos = Inches.of(11.375); + + // Front Right + private static final int kFrontRightDriveMotorId = 21; + private static final int kFrontRightSteerMotorId = 22; + private static final int kFrontRightEncoderId = 23; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(11.375); + private static final Distance kFrontRightYPos = Inches.of(-11.375); + + // Back Left + private static final int kBackLeftDriveMotorId = 1; + private static final int kBackLeftSteerMotorId = 2; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-11.375); + private static final Distance kBackLeftYPos = Inches.of(11.375); + + // Back Right + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 32; + private static final int kBackRightEncoderId = 33; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-11.375); + private static final Distance kBackRightYPos = Inches.of(-11.375); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..f574ff5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,281 @@ +package frc.robot.subsystems; +import java.util.Optional; +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants.DriveMode; +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.Constants; + +import edu.wpi.first.networktables.*; + + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. + * + * Generated by the 2026 Tuner X Swerve Project Generator + * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html + */ +public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.kCW_90deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + private DriveMode m_driveMode = DriveMode.OTHER; + + private NetworkTable positioningNetworkTable; + private NetworkTable driveNetworkTable; + + private final StructPublisher posePub = + NetworkTableInstance.getDefault() + .getStructTopic("/Odometry/RobotPose", Pose2d.struct) + .publish(); + + private Field2d m_field; + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, modules); + m_field = new Field2d(); + SmartDashboard.putData("Field", m_field); + LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME, 0); + positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning"); + driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive"); + + configueAutoBuilder(); + + } + + + private void configueAutoBuilder(){ + + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + throw new RuntimeException("Failed to load PathPlanner RobotConfig", e); + } + + AutoBuilder.configure( + this::getPose, // 当前机器人 Pose + this::resetPose, // 重置里程计 + this::getRobotRelativeSpeeds, // ROBOT RELATIVE speeds + (speeds, feedforwards) -> driveRobotRelative(speeds), // 用 speeds 驱动机器人 + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // X/Y PID + new PIDConstants(5.0, 0.0, 0.0) // Theta PID + ), + config, + () -> false, + this + ); + + } + public Pose2d getPose() { + return getState().Pose; + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return getState().Speeds; + } + + public void driveRobotRelative(ChassisSpeeds speeds) { + ChassisSpeeds scaled = new ChassisSpeeds( + speeds.vxMetersPerSecond, // X 不变 + speeds.vyMetersPerSecond, // Y 不变 + speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转 + ); + + setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) + ); + } + + + + + private static final double kMaxVisionJumpMeters = 1.0; + private static final double kMaxOmegaDegPerSec = 360.0; + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier request) { + return run(() -> this.setControl(request.get())); + } + + public Command applyRequest(Supplier requestSupplier, Supplier driveModeSupplier) { + return run(() -> { + DriveMode mode = driveModeSupplier.get(); + m_driveMode = mode; + this.setControl(requestSupplier.get()); + }); + } + + public void forceUsingLimelightMT2() { + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME); + + if (mt2 == null || mt2.tagCount == 0) { + return; + } + + var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999); + addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); + } + + private void fuseLimelightMT2() { + + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME); + + Pose2d current = getState().Pose; + Pose2d mt2Pose = mt2.pose; + + double distance = current.getTranslation().getDistance(mt2Pose.getTranslation()); + Rotation2d deltaRotation = current.getRotation().minus(mt2Pose.getRotation()); + + positioningNetworkTable.getEntry("CurrentPoseX").setDouble(current.getX()); + positioningNetworkTable.getEntry("CurrentPoseY").setDouble(current.getY()); + positioningNetworkTable.getEntry("MetaTag2PoseX").setDouble(mt2Pose.getX()); + positioningNetworkTable.getEntry("MetaTag2PoseY").setDouble(mt2Pose.getY()); + positioningNetworkTable.getEntry("PoseDistance").setDouble(distance); + positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees())); + + // 1) 没看到 tag：拒绝 + if (mt2 == null || mt2.tagCount == 0) { + positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false); + return; + } + // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿） + double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号 + if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) { + positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false); + System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed"); + return; + } + + // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略） + //Pose2d current = getState().Pose; + if (current.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { + positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false); + System.out.println("NoUsingMetaTag2ForTooChangingPose"); + return; + } + + // 4) 设置测量噪声（示例：theta 基本不信任就给极大） + // 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调 + var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999); + + // 5) 融合：注意 timestampSeconds 的处理 + addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); + positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); + } + + + @Override + public void periodic() { + Pose2d pose = getState().Pose; + m_field.setRobotPose(pose); + posePub.set(pose); + driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees()); + driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name()); + driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC); + + // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0 + LimelightHelpers.SetRobotOrientation( + Constants.Limelight.LIMELIGHT_NAME, + pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定 + 0, 0, 0, 0, 0 + ); + + fuseLimelightMT2(); + + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + m_hasAppliedOperatorPerspective = true; + }); + } + } + + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this method + * will continue to apply to future measurements until a subsequent call to + * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement + * in the form [x, y, theta]ᵀ, with units in meters and radians. + */ + @Override + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } + + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestampSeconds The timestamp of the pose in seconds. + * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + */ + @Override + public Optional samplePoseAt(double timestampSeconds) { + return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java new file mode 100644 index 0000000..dcaa232 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class IntakerSubsystem extends SubsystemBase{ + //定义intakerotater&intakegetter + private TalonFX intakerotater; + private TalonFX intakegetter; + + //PID + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final PositionVoltage positionRequest = new PositionVoltage(0); + + private double intakeRotaterTargetPosition = 0.0; + + public IntakerSubsystem() { + //初始化rotater&getter + intakegetter = new TalonFX(47); + intakerotater = new TalonFX(48); + intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs); + intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs); + } + + public void setIntakerRotaterPosition(double targetPosition) { + //目标position + if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) { + targetPosition = Constants.Intaker.intakeRotaterUpLimit; // 限制最大上限 + } else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) { + targetPosition = Constants.Intaker.intakeRotaterDownLimit; // 限制最小下限 + } + + intakeRotaterTargetPosition = targetPosition; + + intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID） + } + + // 获取intakerotater当前角度 + public double getIntakerotaterPosition() { + return intakerotater.getPosition().getValueAsDouble(); + } + + public boolean isIntakerotaterAtTarget() { + // 到位判断容差 + double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition()); + return error <= Constants.Intaker.positionToleranceRotations; + } + + public void stopintakerotater() { + intakerotater.setControl(velocityRequest.withVelocity(0)); + // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI） + } + + //intakegetter的on/off + public void setIntakeGetterOn(boolean on) { + if (on) { + intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed)); + } + else { + intakegetter.setControl(velocityRequest.withVelocity(0)); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..3ac4f9e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,151 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class LEDSubsystem extends SubsystemBase { + + public enum Mode { + OFF, + SOLID, + BREATH, + RAINBOW + } + + private final AddressableLED led; + private final AddressableLEDBuffer buffer; + private final Timer timer = new Timer(); + + // 左右灯带分段（左 0~71，右 72~143） + private final int leftStart = 0; + private final int leftLen; + private final int rightStart; + private final int rightLen; + + private Mode leftMode = Mode.OFF; + private Mode rightMode = Mode.OFF; + + // 左右颜色 + private int lr = 0, lg = 0, lb = 0; + private int rr = 0, rg = 0, rb = 0; + + // 动画参数 + private double leftBreathPeriodSec = 2.0; + private double rightBreathPeriodSec = 2.0; + private int leftRainbowOffset = 0; + private int rightRainbowOffset = 0; + + /** + * @param pwmPort PWM 端口（只能用一个！） + * @param leftCount 左灯数量 + * @param rightCount 右灯数量 + */ + public LEDSubsystem(int pwmPort, int leftCount, int rightCount) { + this.leftLen = leftCount; + this.rightLen = rightCount; + this.rightStart = leftStart + leftLen; + + led = new AddressableLED(pwmPort); + buffer = new AddressableLEDBuffer(leftLen + rightLen); + + led.setLength(buffer.getLength()); + led.start(); + + timer.start(); + } + + /* ====================== */ + /* 对外API */ + /* ====================== */ + + public void offLeft() { leftMode = Mode.OFF; } + public void offRight() { rightMode = Mode.OFF; } + public void offAll() { leftMode = Mode.OFF; rightMode = Mode.OFF; } + + public void solidLeft(int r, int g, int b) { + lr = clamp8(r); lg = clamp8(g); lb = clamp8(b); + leftMode = Mode.SOLID; + } + + public void solidRight(int r, int g, int b) { + rr = clamp8(r); rg = clamp8(g); rb = clamp8(b); + rightMode = Mode.SOLID; + } + + public void breathLeft(int r, int g, int b, double periodSeconds) { + lr = clamp8(r); lg = clamp8(g); lb = clamp8(b); + leftBreathPeriodSec = Math.max(0.2, periodSeconds); + leftMode = Mode.BREATH; + } + + public void breathRight(int r, int g, int b, double periodSeconds) { + rr = clamp8(r); rg = clamp8(g); rb = clamp8(b); + rightBreathPeriodSec = Math.max(0.2, periodSeconds); + rightMode = Mode.BREATH; + } + + public void rainbowLeft() { leftMode = Mode.RAINBOW; } + public void rainbowRight() { rightMode = Mode.RAINBOW; } + public void rainbowAll() { leftMode = Mode.RAINBOW; rightMode = Mode.RAINBOW; } + + @Override + public void periodic() { + if(!Constants.LEDUsing) {led.close(); return;} + // 左段 + switch (leftMode) { + case OFF -> setRange(leftStart, leftLen, 0, 0, 0); + case SOLID -> setRange(leftStart, leftLen, lr, lg, lb); + case BREATH -> updateBreath(leftStart, leftLen, lr, lg, lb, leftBreathPeriodSec); + case RAINBOW -> updateRainbow(leftStart, leftLen, true); + } + + // 右段 + switch (rightMode) { + case OFF -> setRange(rightStart, rightLen, 0, 0, 0); + case SOLID -> setRange(rightStart, rightLen, rr, rg, rb); + case BREATH -> updateBreath(rightStart, rightLen, rr, rg, rb, rightBreathPeriodSec); + case RAINBOW -> updateRainbow(rightStart, rightLen, false); + } + + led.setData(buffer); + } + + /* ====================== */ + /* 内部实现 */ + /* ====================== */ + + private void setRange(int start, int len, int r, int g, int b) { + for (int i = 0; i < len; i++) { + buffer.setRGB(start + i, r, g, b); + } + } + + private void updateBreath(int start, int len, int r, int g, int b, double periodSec) { + double t = timer.get(); + double phase = (t % periodSec) / periodSec; // 0..1 + double brightness = 0.5 * (1.0 + Math.sin(2.0 * Math.PI * phase)); // 0..1 + int rr = (int) (r * brightness); + int gg = (int) (g * brightness); + int bb = (int) (b * brightness); + setRange(start, len, rr, gg, bb); + } + + private void updateRainbow(int start, int len, boolean isLeft) { + int offset = isLeft ? leftRainbowOffset : rightRainbowOffset; + + for (int i = 0; i < len; i++) { + int hue = (offset + (i * 180 / len)) % 180; + buffer.setHSV(start + i, hue, 255, 128); + } + + if (isLeft) leftRainbowOffset = (leftRainbowOffset + 2) % 180; + else rightRainbowOffset = (rightRainbowOffset + 2) % 180; + } + + private static int clamp8(int v) { + return Math.max(0, Math.min(255, v)); + } +} diff --git a/src/main/java/frc/robot/subsystems/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/LimelightHelpers.java new file mode 100644 index 0000000..acf0a2b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightHelpers.java @@ -0,0 +1,1945 @@ +//LimelightHelpers v1.14 (REQUIRES LLOS 2026.0 OR LATER) + +package frc.robot.subsystems; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; + +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.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.Arrays; +import java.util.Map; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; +import edu.wpi.first.net.PortForwarder; + + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Represents hardware statistics from the Limelight. + */ + public static class HardwareReport { + @JsonProperty("cid") + public String cameraId; + + @JsonProperty("cpu") + public double cpuUsage; + + @JsonProperty("dfree") + public double diskFree; + + @JsonProperty("dtot") + public double diskTotal; + + @JsonProperty("ram") + public double ramUsage; + + @JsonProperty("temp") + public double temperature; + + public HardwareReport() { + } + } + + /** + * Represents IMU data from the JSON results. + */ + public static class IMUResults { + @JsonProperty("data") + public double[] data; + + @JsonProperty("quat") + public double[] quaternion; + + @JsonProperty("yaw") + public double yaw; + + // Parsed from data array + public double robotYaw; + public double roll; + public double pitch; + public double rawYaw; + public double gyroZ; + public double gyroX; + public double gyroY; + public double accelZ; + public double accelX; + public double accelY; + + public IMUResults() { + data = new double[0]; + quaternion = new double[4]; + } + + public void parseDataArray() { + if (data != null && data.length >= 10) { + robotYaw = data[0]; + roll = data[1]; + pitch = data[2]; + rawYaw = data[3]; + gyroZ = data[4]; + gyroX = data[5]; + gyroY = data[6]; + accelZ = data[7]; + accelX = data[8]; + accelY = data[9]; + } + } + } + + /** + * Represents capture rewind buffer statistics. + */ + public static class RewindStats { + @JsonProperty("bufferUsage") + public double bufferUsage; + + @JsonProperty("enabled") + public int enabled; + + @JsonProperty("flushing") + public int flushing; + + @JsonProperty("frameCount") + public int frameCount; + + @JsonProperty("latpen") + public int latencyPenalty; + + @JsonProperty("storedSeconds") + public double storedSeconds; + + public RewindStats() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("ts_nt") + public long timestamp_nt; + + @JsonProperty("ts_sys") + public long timestamp_sys; + + @JsonProperty("ts_us") + public long timestamp_us; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("pTYPE") + public String pipelineType; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txnc") + public double tx_nocrosshair; + + @JsonProperty("tync") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("botpose_orb") + public double[] botpose_orb; + + @JsonProperty("botpose_orb_wpiblue") + public double[] botpose_orb_wpiblue; + + @JsonProperty("botpose_orb_wpired") + public double[] botpose_orb_wpired; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + @JsonProperty("hw") + public HardwareReport hardware; + + @JsonProperty("imu") + public IMUResults imuResults; + + @JsonProperty("rewind") + public RewindStats rewindStats; + + @JsonProperty("PythonOut") + public double[] pythonOutput; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + botpose_orb = new double[6]; + botpose_orb_wpiblue = new double[6]; + botpose_orb_wpired = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + pythonOutput = new double[0]; + pipelineType = ""; + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id && + Double.compare(txnc, other.txnc) == 0 && + Double.compare(tync, other.tync) == 0 && + Double.compare(ta, other.ta) == 0 && + Double.compare(distToCamera, other.distToCamera) == 0 && + Double.compare(distToRobot, other.distToRobot) == 0 && + Double.compare(ambiguity, other.ambiguity) == 0; + } + + } + + /** + * Represents a Limelight Raw Target/Contour result from Limelight's NetworkTables output. + */ + public static class RawTarget { + public double txnc = 0; + public double tync = 0; + public double ta = 0; + + public RawTarget(double txnc, double tync, double ta) { + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawTarget other = (RawTarget) obj; + return Double.compare(txnc, other.txnc) == 0 && + Double.compare(tync, other.tync) == 0 && + Double.compare(ta, other.ta) == 0; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return new PoseEstimate(); + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + RawFiducial[] rawFiducials; + + if (poseArray.length != expectedTotalVals) { + // Array size mismatch - return empty array instead of null-filled array + rawFiducials = new RawFiducial[0]; + } else { + rawFiducials = new RawFiducial[tagCount]; + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Gets the raw target contours from NetworkTables. + * Returns ungrouped contours in normalized screen space (-1 to 1). + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawTarget objects containing up to 3 contours + */ + public static RawTarget[] getRawTargets(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawtargets"); + var rawTargetArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 3; + if (rawTargetArray.length % valsPerEntry != 0) { + return new RawTarget[0]; + } + + int numTargets = rawTargetArray.length / valsPerEntry; + RawTarget[] rawTargets = new RawTarget[numTargets]; + + for (int i = 0; i < numTargets; i++) { + int baseIndex = i * valsPerEntry; + double txnc = extractArrayEntry(rawTargetArray, baseIndex); + double tync = extractArrayEntry(rawTargetArray, baseIndex + 1); + double ta = extractArrayEntry(rawTargetArray, baseIndex + 2); + + rawTargets[i] = new RawTarget(txnc, tync, ta); + } + + return rawTargets; + } + + /** + * Gets the corner coordinates of detected targets from NetworkTables. + * Requires "send contours" to be enabled in the Limelight Output tab. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of doubles containing corner coordinates [x0, y0, x1, y1, ...] + */ + public static double[] getCornerCoordinates(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tcornxy"); + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + /** + * Gets the average color under the crosshair region as a 3-element array. + * @param limelightName Name of the Limelight camera + * @return Array containing [Blue, Green, Red] color values (BGR order) + */ + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + /** + * Gets the Limelight heartbeat value. Increments once per frame, allowing you to detect if the Limelight is connected and alive. + * @param limelightName Name of the Limelight camera + * @return Heartbeat value that increments each frame + */ + public static double getHeartbeat(String limelightName) { + return getLimelightNTDouble(limelightName, "hb"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets the keystone modification for the crop window. + * @param limelightName Name of the Limelight camera + * @param horizontal Horizontal keystone value (-0.95 to 0.95) + * @param vertical Vertical keystone value (-0.95 to 0.95) + */ + public static void setKeystone(String limelightName, double horizontal, double vertical) { + double[] entries = new double[2]; + entries[0] = horizontal; + entries[1] = vertical; + setLimelightNTDoubleArray(limelightName, "keystone_set", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Triggers a snapshot capture via NetworkTables by incrementing the snapshot counter. + * Rate-limited to once per 10 frames on the Limelight. + * @param limelightName Name of the Limelight camera + */ + public static void triggerSnapshot(String limelightName) { + double current = getLimelightNTDouble(limelightName, "snapshot"); + setLimelightNTDouble(limelightName, "snapshot", current + 1); + } + + /** + * Enables or pauses the rewind buffer recording. + * @param limelightName Name of the Limelight camera + * @param enabled True to enable recording, false to pause + */ + public static void setRewindEnabled(String limelightName, boolean enabled) { + setLimelightNTDouble(limelightName, "rewind_enable_set", enabled ? 1 : 0); + } + + /** + * Triggers a rewind capture with the specified duration. + * Maximum duration is 165 seconds. Rate-limited on the Limelight. + * @param limelightName Name of the Limelight camera + * @param durationSeconds Duration of rewind capture in seconds (max 165) + */ + public static void triggerRewindCapture(String limelightName, double durationSeconds) { + double[] currentArray = getLimelightNTDoubleArray(limelightName, "capture_rewind"); + double counter = (currentArray.length > 0) ? currentArray[0] : 0; + double[] entries = new double[2]; + entries[0] = counter + 1; + entries[1] = Math.min(durationSeconds, 165); + setLimelightNTDoubleArray(limelightName, "capture_rewind", entries); + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + String jsonString = getJSONDump(limelightName); + if (jsonString == null || jsonString.isEmpty() || jsonString.isBlank()) { + results.error = "lljson error: empty json"; + } else { + results = mapper.readValue(jsonString, LimelightResults.class); + if (results.imuResults != null) { + results.imuResults.parseDataArray(); + } + } + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } + + /** + * Sets up port forwarding for a Limelight 3A/3G connected via USB. + * This allows access to the Limelight web interface and video stream + * when connected to the robot over USB. + * + * For usbIndex 0: ports 5800-5809 forward to 172.29.0.1 + * For usbIndex 1: ports 5810-5819 forward to 172.29.1.1 + * etc. + * + * Call this method once during robot initialization. + * To access the interface of the camera with usbIndex0, you would go to roboRIO-(teamnum)-FRC.local:5801. Port 5811 for usb index 1 + * + * @param usbIndex The USB index of the Limelight (0, 1, 2, etc.) + */ + public static void setupPortForwardingUSB(int usbIndex) { + String ip = "172.29." + usbIndex + ".1"; + int basePort = 5800 + (usbIndex * 10); + + for (int i = 0; i < 10; i++) { + PortForwarder.add(basePort + i, ip, 5800 + i); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..170789d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,171 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class ShooterSubsystem extends SubsystemBase { + // 定义飞轮、传动和背板电机 + public TalonFX flywheelMotorLeft; + private TalonFX flywheelMotorRight; + public TalonFX conveyorMotor; + public TalonFXS backboardMotor; + public Encoder backboardEncoder; + + // PID 控制器，控制背板角度 + public ProfiledPIDController backboardPID; + private Follower flyWheelFollower; + + private NetworkTable shooterNetworkTable; + + private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + + public enum Mode{ + Inalyzed, + UnInalyed, + Inalyzing; + } + public Mode currentBackboardInalyzeMode; + // 初始化各个部件 + public ShooterSubsystem() { + // 初始化飞轮电机 + flywheelMotorLeft = new TalonFX(41); // 41是飞轮left的CAN ID + flywheelMotorRight = new TalonFX(42); // 42是飞轮right的CAN ID + flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs); + flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs); + // 初始化传动电机 + conveyorMotor = new TalonFX(43); + conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs); + + var configs1 = new MotorOutputConfigs(); + configs1.Inverted = InvertedValue.Clockwise_Positive; + flywheelMotorLeft.getConfigurator().apply(configs1); + flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); + + var configs2 = new MotorOutputConfigs(); + configs2.Inverted = InvertedValue.Clockwise_Positive; + conveyorMotor.getConfigurator().apply(configs2); + + // 初始化背板电机和编码器 + backboardMotor = new TalonFXS(44); + backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs); + backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X); + backboardEncoder.setSamplesToAverage(5); + backboardEncoder.setMinRate(1.0); + backboardEncoder.setDistancePerPulse(1); + + // 初始化背板的PID控制器 + backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD, + new Constraints(10000,100000)); + + backboardPID.setTolerance(10.0); + + shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter"); + + currentBackboardInalyzeMode = Mode.UnInalyed; + } + + public void setRightFollowLeft(){ + flywheelMotorRight.setControl(flyWheelFollower); + } + + public void applyLeftConfigurationToRight(){ + Slot0Configs configs = new Slot0Configs(); + flywheelMotorLeft.getConfigurator().refresh(configs); + flywheelMotorRight.getConfigurator().apply(configs); + } + + + // 设置飞轮的速度 + public void setFlywheelSpeedByRPS(double targetSpeed) { + flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒 + setRightFollowLeft(); + // System.out.println("set flywheel "+targetSpeed); + } + + // 设置传动的速度 + public void setConveyorSpeedByRPS(double speed) { + conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度 + } + + //调试用 + public void setBackboardSpeedByRPS(double speed){ + backboardMotor.setControl(velocityRequest.withVelocity(speed)); + } + + // 设置背板的目标位置，通过PID控制器来调整 范围0-3000 + public void setBackboardPosition(double targetPosition) { + // 限制目标位置在上下限之间 + if (targetPosition > Constants.Shooter.backboardUpLimit) { + targetPosition = Constants.Shooter.backboardUpLimit; // 限制最大上限 + } else if (targetPosition < Constants.Shooter.backboardDownLimit) { + targetPosition = Constants.Shooter.backboardDownLimit; // 限制最小下限 + } + backboardPID.setGoal(targetPosition); + } + public void outputBackboard(){ + + // 使用PID控制器来计算背板的运动速度 + double pidOutput = backboardPID.calculate(getBackboardPosition()); + pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围在-1到1之间 + shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput); + // // 设置背板电机的速度，根据PID输出进行调整 + setBackboardSpeedByRPS(pidOutput); + } + // 停止所有电机 + public void stopMotors() { + flywheelMotorLeft.set(0); + flywheelMotorRight.set(0); + conveyorMotor.set(0); + backboardMotor.set(0); + } + + // 获取背板当前角度 + public double getBackboardPosition() { + return backboardEncoder.getDistance(); // 返回背板电机编码器的当前角度 + } + + // 检查背板是否到达目标角度 + public boolean isBackboardAtTarget() { + return backboardPID.atGoal(); // 判断背板是否到达目标角度 + } + + @Override + public void periodic() { + if(currentBackboardInalyzeMode != Mode.Inalyzing){ + backboardPID.calculate(getBackboardPosition()); + if(isBackboardAtTarget()){ + backboardMotor.set(0); + }else{ + outputBackboard(); + } + } + shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble()); + shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble()); + shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble()); + shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble()); + shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); + shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); + shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); + shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance()); + shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name()); + } + + private double getDistance(){ + //获得车子距离Hub的距离 + return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); + } +} diff --git a/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java b/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java new file mode 100644 index 0000000..47132a7 --- /dev/null +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java @@ -0,0 +1,92 @@ +package frc.robot.tuning; +import java.util.function.Function; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.networktables.*; + +public class ConfigTalonFXMotorTuner { + private final TalonFX motor; + private final String motorName; + private final Slot0Configs motorSlot0Configs; + private final NetworkTable table; + + // ---- Subscribers：读取 Elastic 改过的新值 ---- + private final BooleanSubscriber enableSub; + private final DoubleSubscriber KpSub; + private final DoubleSubscriber KiSub; + private final DoubleSubscriber KdSub; + private final DoubleSubscriber KvSub; + private final DoubleSubscriber KaSub; + private final DoubleSubscriber KsSub; + + private final BooleanSubscriber enableRuningSub; + private final DoubleSubscriber runningSpeedRPS; + private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0; + + private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) { + this.motor = motor_; + this.motorName = motorName_; + this.motorSlot0Configs = motorSlot0Configs_; + + this.table = NetworkTableInstance.getDefault().getTable("Tuning/" + motorName); + + table.getBooleanTopic(motorName+"Enable").publish().set(false); + table.getDoubleTopic(motorName+"kP").publish().set(motorSlot0Configs.kP);; + table.getDoubleTopic(motorName+"kI").publish().set(motorSlot0Configs.kI); + table.getDoubleTopic(motorName+"kD").publish().set(motorSlot0Configs.kD); + table.getDoubleTopic(motorName+"kV").publish().set(motorSlot0Configs.kV); + table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA); + table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS); + + table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);; + table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);; + + this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false); + this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP); + this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI); + this.KdSub = table.getDoubleTopic(motorName+"kD").subscribe(motorSlot0Configs.kD); + this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV); + this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA); + this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS); + this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false); + this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0); + } + + public void periodic(Runnable func) { + if (!enableSub.get()) return; + double kp = KpSub.get(); + double ki = KiSub.get(); + double kd = KdSub.get(); + double kv = KvSub.get(); + double ka = KaSub.get(); + double ks = KsSub.get(); + + if (!(kp == lastKp && ki == lastKi && kd == lastKd && kv == lastKv && ka == lastKa && ks == lastKs)) { + + Slot0Configs slot0 = new Slot0Configs() + .withKP(kp).withKI(ki).withKD(kd).withKV(kv).withKA(ka).withKS(ks); + + motor.getConfigurator().apply(slot0); + System.out.println("applyed settings: "+motorName); + + lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks; + + if(func!=null){ + func.run(); + } + } + + if (enableRuningSub.get()) {; + double speed = runningSpeedRPS.get(); + motor.setControl(velocityRequest.withVelocity(speed)); + // System.out.println("running motor with speed: "+speed+" on motor: "+motorName); + }else{ + motor.setControl(velocityRequest.withVelocity(0)); + } + } + + +} diff --git a/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java new file mode 100644 index 0000000..d409418 --- /dev/null +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java @@ -0,0 +1,86 @@ +package frc.robot.tuning; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFXS; + +import edu.wpi.first.networktables.*; + +public class ConfigTalonFXSMotorTuner { + private final TalonFXS motor; + private final String motorName; + private final Slot0Configs motorSlot0Configs; + private final NetworkTable table; + + // ---- Subscribers：读取 Elastic 改过的新值 ---- + private final BooleanSubscriber enableSub; + private final DoubleSubscriber KpSub; + private final DoubleSubscriber KiSub; + private final DoubleSubscriber KdSub; + private final DoubleSubscriber KvSub; + private final DoubleSubscriber KaSub; + private final DoubleSubscriber KsSub; + + private final BooleanSubscriber enableRuningSub; + private final DoubleSubscriber runningSpeedRPS; + private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0; + + private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) { + this.motor = motor_; + this.motorName = motorName_; + this.motorSlot0Configs = motorSlot0Configs_; + + this.table = NetworkTableInstance.getDefault().getTable("Tuning/" + motorName); + table.getBooleanTopic(motorName+"Enable").publish().set(false); + table.getDoubleTopic(motorName+"kP").publish().set(motorSlot0Configs.kP); + table.getDoubleTopic(motorName+"kI").publish().set(motorSlot0Configs.kI); + table.getDoubleTopic(motorName+"kD").publish().set(motorSlot0Configs.kD); + table.getDoubleTopic(motorName+"kV").publish().set(motorSlot0Configs.kV); + table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA); + table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS); + + table.getBooleanTopic(motorName+"EnableRunning").publish().set(false); + table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0); + + this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false); + this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP); + this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI); + this.KdSub = table.getDoubleTopic(motorName+"kD").subscribe(motorSlot0Configs.kD); + this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV); + this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA); + this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS); + this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false); + this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0); + } + + public void periodic() { + if (!enableSub.get()) return; + + double kp = KpSub.get(); + double ki = KiSub.get(); + double kd = KdSub.get(); + double kv = KvSub.get(); + double ka = KaSub.get(); + double ks = KsSub.get(); + + if (!(kp == lastKp && ki == lastKi && kd == lastKd && kv == lastKv && ka == lastKa && ks == lastKs)) { + + Slot0Configs slot0 = new Slot0Configs() + .withKP(kp).withKI(ki).withKD(kd).withKV(kv).withKA(ka).withKS(ks); + + motor.getConfigurator().apply(slot0); + System.out.println("applyed settings: "+motorName); + + lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks; + } + + if (enableRuningSub.get()) {; + double speed = runningSpeedRPS.get(); + motor.setControl(velocityRequest.withVelocity(speed)); + // System.out.println("running motor with speed: "+speed+" on motor: "+motorName); + }else{ + motor.setControl(velocityRequest.withVelocity(0)); + } + } +} diff --git a/src/main/java/frc/robot/tuning/DriveGainsTuner.java b/src/main/java/frc/robot/tuning/DriveGainsTuner.java new file mode 100644 index 0000000..2370a04 --- /dev/null +++ b/src/main/java/frc/robot/tuning/DriveGainsTuner.java @@ -0,0 +1,68 @@ +package frc.robot.tuning; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.networktables.*; + +public class DriveGainsTuner { + private final NetworkTable table = + NetworkTableInstance.getDefault().getTable("Tuning/Drive"); + + // ---- Publishers：让 Elastic 能“看到”这些键，并给初值 ---- + private final BooleanPublisher enablePub = + table.getBooleanTopic("Enable").publish(); + + private final DoublePublisher kpPub = table.getDoubleTopic("kP").publish(); + private final DoublePublisher kiPub = table.getDoubleTopic("kI").publish(); + private final DoublePublisher kdPub = table.getDoubleTopic("kD").publish(); + private final DoublePublisher kvPub = table.getDoubleTopic("kV").publish(); + + // ---- Subscribers：读取 Elastic 改过的新值 ---- + private final BooleanSubscriber enableSub = + table.getBooleanTopic("Enable").subscribe(false); + + private final DoubleSubscriber kpSub = table.getDoubleTopic("kP").subscribe(0.01); + private final DoubleSubscriber kiSub = table.getDoubleTopic("kI").subscribe(0.0); + private final DoubleSubscriber kdSub = table.getDoubleTopic("kD").subscribe(0.0); + private final DoubleSubscriber kvSub = table.getDoubleTopic("kV").subscribe(0.124); + + private double lastKP = Double.NaN, lastKI = Double.NaN, lastKD = Double.NaN, lastKV = Double.NaN; + + private final TalonFX[] driveMotors; + + public DriveGainsTuner(SwerveDrivetrain drivetrain) { + // 发布初始值（这一步做完 Elastic 就能看到这些键） + enablePub.set(false); + kpPub.set(0.01); + kiPub.set(0.0); + kdPub.set(0.0); + kvPub.set(0.124); + + var modules = drivetrain.getModules(); + driveMotors = new TalonFX[modules.length]; + for (int i = 0; i < modules.length; i++) { + driveMotors[i] = (TalonFX) modules[i].getDriveMotor(); + } + } + + public void periodic() { + if (!enableSub.get()) return; + + double kp = kpSub.get(); + double ki = kiSub.get(); + double kd = kdSub.get(); + double kv = kvSub.get(); + + if (kp == lastKP && ki == lastKI && kd == lastKD && kv == lastKV) return; + + Slot0Configs slot0 = new Slot0Configs() + .withKP(kp).withKI(ki).withKD(kd).withKV(kv); + + for (TalonFX m : driveMotors) { + m.getConfigurator().apply(slot0); + } + + lastKP = kp; lastKI = ki; lastKD = kd; lastKV = kv; + } +} diff --git a/src/main/java/frc/robot/utils/LedBindings.java b/src/main/java/frc/robot/utils/LedBindings.java new file mode 100644 index 0000000..8bd6e80 --- /dev/null +++ b/src/main/java/frc/robot/utils/LedBindings.java @@ -0,0 +1,32 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import frc.robot.subsystems.LEDSubsystem; + +public final class LedBindings { + private LedBindings() {} + + public static void bindModeIndicators(LEDSubsystem leds) { + // Disabled：左右红色呼吸（允许在禁用状态运行） + RobotModeTriggers.disabled().onTrue( + Commands.runOnce(() -> { + leds.breathLeft(255, 0, 0, 1.0); + leds.breathRight(255, 0, 0, 1.0); + }, leds).ignoringDisable(true) + ); + + // Teleop：左右彩虹 + RobotModeTriggers.teleop().onTrue( + Commands.runOnce(leds::rainbowAll, leds) + ); + + // Auto：左右蓝色常亮 + RobotModeTriggers.autonomous().onTrue( + Commands.runOnce(() -> { + leds.solidLeft(0, 0, 255); + leds.solidRight(0, 0, 255); + }, leds) + ); + } +} diff --git a/tuner-project.json b/tuner-project.json new file mode 100644 index 0000000..c475088 --- /dev/null +++ b/tuner-project.json @@ -0,0 +1,232 @@ +{ + "Version": "1.0.0.0", + "LastState": 11, + "Modules": [ + { + "ModuleName": "Front Left", + "ModuleId": 0, + "Encoder": { + "Id": 13, + "Name": "Mod1 Cancoder", + "Model": "CANCoder vers. H", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": null + }, + "SteerMotor": { + "Id": 12, + "Name": "Mod1 Angle", + "Model": "Talon FX", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "DriveMotor": { + "Id": 11, + "Name": "Mod1 Drive", + "Model": "Talon FX vers. C", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "IsEncoderInverted": false, + "IsSteerInverted": false, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": -0.412841796875, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 12, + "ValidatedDriveId": 11, + "ValidatedEncoderId": 13 + }, + { + "ModuleName": "Front Right", + "ModuleId": 1, + "Encoder": { + "Id": 23, + "Name": "Mod2 Cancoder", + "Model": "CANCoder vers. H", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": null + }, + "SteerMotor": { + "Id": 22, + "Name": "Mod2 Angle", + "Model": "Talon FX", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "DriveMotor": { + "Id": 21, + "Name": "Mod2 Drive", + "Model": "Talon FX vers. C", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "IsEncoderInverted": false, + "IsSteerInverted": false, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": -0.012939453125, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 22, + "ValidatedDriveId": 21, + "ValidatedEncoderId": 23 + }, + { + "ModuleName": "Back Left", + "ModuleId": 2, + "Encoder": { + "Id": 3, + "Name": "Mod0 Cancoder", + "Model": "CANCoder", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": null + }, + "SteerMotor": { + "Id": 2, + "Name": "Mod0 Angle", + "Model": "Talon FX", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "DriveMotor": { + "Id": 1, + "Name": "Mod0 Drive", + "Model": "Talon FX vers. C", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "IsEncoderInverted": false, + "IsSteerInverted": false, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": 0.07275390625, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 2, + "ValidatedDriveId": 1, + "ValidatedEncoderId": 3 + }, + { + "ModuleName": "Back Right", + "ModuleId": 3, + "Encoder": { + "Id": 33, + "Name": "Mod3 Cancoder", + "Model": "CANCoder vers. H", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": null + }, + "SteerMotor": { + "Id": 32, + "Name": "Mod3 Angle", + "Model": "Talon FX", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "DriveMotor": { + "Id": 31, + "Name": "Mod3 Drive", + "Model": "Talon FX vers. C", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + } + }, + "IsEncoderInverted": false, + "IsSteerInverted": false, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": -0.4892578125, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 32, + "ValidatedDriveId": 31, + "ValidatedEncoderId": 33 + } + ], + "SwerveOptions": { + "Gyro": { + "Id": 0, + "Name": "Pigeon 2 vers. S (Device ID 0)", + "Model": "Pigeon 2 vers. S", + "CANbus": "rio", + "CANbusFriendly": "", + "SelectedMotorType": null + }, + "IsValidGyroCANbus": true, + "VerticalTrackSizeInches": 22.75, + "HorizontalTrackSizeInches": 22.75, + "WheelRadiusInches": 1.81, + "IsLeftSideInverted": false, + "IsRightSideInverted": true, + "SwerveModuleType": 6, + "SwerveModuleConfiguration": { + "ModuleBrand": 6, + "DriveRatio": 5.902777777777778, + "SteerRatio": 12.8, + "CouplingRatio": 3.125, + "CustomName": "L2+" + }, + "HasVerifiedSteer": true, + "SelectedModuleManufacturer": "Swerve Drive Specialties (SDS)", + "HasVerifiedDrive": true, + "IsValidConfiguration": true + }, + "TeamNumber": 6494, + "schema_version": "1" +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json new file mode 100644 index 0000000..f72fa41 --- /dev/null +++ b/vendordeps/PathplannerLib-2026.1.2.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2026.1.2.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "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/Phoenix6-26.1.1.json b/vendordeps/Phoenix6-26.1.1.json new file mode 100644 index 0000000..7a0eca0 --- /dev/null +++ b/vendordeps/Phoenix6-26.1.1.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.1.1.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.1.1", + "frcYear": "2026", + "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-frc2026-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-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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": "26.1.1", + "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/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..d90630e --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,39 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2026", + "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": [ + "linuxsystemcore", + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +}