--- /dev/null
+# Auto detect text files and perform LF normalization
+* text=auto
--- /dev/null
+# 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
--- /dev/null
+{
+ // 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,
+ }
+ ]
+}
--- /dev/null
+{
+ "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.*",
+ "org.wpilib.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.*",
+ "org.wpilib.math.proto.*",
+ "org.wpilib.math.**.proto.*",
+ "org.wpilib.math.**.struct.*",
+ ],
+ "java.dependency.enableDependencyCheckup": false
+}
--- /dev/null
+{
+ "enableCppIntellisense": false,
+ "currentLanguage": "java",
+ "projectYear": "2027_alpha5",
+ "teamNumber": 972
+}
\ No newline at end of file
--- /dev/null
+Copyright (c) 2009-2026 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.
--- /dev/null
+plugins {
+ id "java"
+ id "org.wpilib.GradleRIO" version "2027.0.0-alpha-6"
+ id "com.gradleup.shadow" version "9.3.0"
+}
+
+java {
+ sourceCompatibility = JavaVersion.VERSION_25
+ targetCompatibility = JavaVersion.VERSION_25
+}
+
+def ROBOT_MAIN_CLASS = "first.Main"
+
+// Define my targets (SystemCore) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+ targets {
+ systemcore(getTargetTypeClass('SystemCore')) {
+ // 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.wpilib.getTeamNumber()
+ // Use the default systemcore host name. This must be called after setting team
+ // as happens on the line above
+ useDefaultSystemcoreHostName()
+ debug = project.wpilib.getDebugOrDefault(false)
+
+ artifacts {
+ // First part is artifact name, 2nd is artifact type
+ // getTargetTypeClass is a shortcut to get the class type using a string
+
+ wpilibJava(getArtifactTypeClass('WPILibJavaArtifact')) {
+ }
+
+ // Static files artifact
+ wpilibStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+ files = project.fileTree('src/main/deploy')
+ directory = '/home/systemcore/deploy'
+ deleteOldFiles = false // Change to true to delete files on systemcore that no
+ // longer exist in deploy directory of this project
+ }
+ }
+ }
+ }
+}
+
+def deployArtifact = deploy.targets.systemcore.artifacts.wpilibJava
+
+// 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()
+
+ systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore)
+ systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore)
+
+ systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore)
+ systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore)
+
+ 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/shaded jar')
+// in order to make them all available at runtime and merging service files to make JSON work.
+// Also adding the manifest so WPILib knows where to look for our Robot Class.
+shadowJar {
+ mergeServiceFiles()
+ from('src') { into 'backup/src' }
+ from('vendordeps') { into 'backup/vendordeps' }
+ from('build.gradle') { into 'backup' }
+ manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+ duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = shadowJar
+wpi.java.configureExecutableTasks(shadowJar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+ options.compilerArgs.add '-XDstringConcat=inline'
+}
--- /dev/null
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-9.4.1-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
--- /dev/null
+#!/bin/sh
+
+#
+# Copyright © 2015 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/2d6327017519d23b96af35865dc997fcb544fb40/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\n' "$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
+
+
+
+# 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" )
+
+ 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, 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" \
+ -jar "$APP_HOME/gradle/wrapper/gradle-wrapper.jar" \
+ "$@"
+
+# 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" "$@"
--- /dev/null
+@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
+
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %*
+
+: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
--- /dev/null
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+ repositories {
+ String wpilibYear = '2027_alpha5'
+ File wpilibHome
+ if (OperatingSystem.current().isWindows()) {
+ String publicFolder = System.getenv('PUBLIC')
+ if (publicFolder == null) {
+ publicFolder = "C:\\Users\\Public"
+ }
+ def homeRoot = new File(publicFolder, "wpilib")
+ wpilibHome = new File(homeRoot, wpilibYear)
+ } else {
+ def userFolder = System.getProperty("user.home")
+ def homeRoot = new File(userFolder, "wpilib")
+ wpilibHome = new File(homeRoot, wpilibYear)
+ }
+ def wpilibHomeMaven = new File(wpilibHome, 'maven')
+ maven {
+ name = 'wpilibHome'
+ url = wpilibHomeMaven
+ }
+ mavenLocal()
+ gradlePluginPortal()
+ }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
--- /dev/null
+{
+ "name":"project",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+ "expressions":{},
+ "poses":{
+ "shootPos":{
+ "x":{
+ "exp":"4.036591053009033 m",
+ "val":4.036591053009033
+ },
+ "y":{
+ "exp":"7.649001598358154 m",
+ "val":7.649001598358154
+ },
+ "heading":{
+ "exp":"0 rad",
+ "val":0.0
+ }
+ }
+ }
+ },
+ "config":{
+ "frontLeft":{
+ "x":{
+ "exp":"20.75 in",
+ "val":0.52705
+ },
+ "y":{
+ "exp":"20.75 in",
+ "val":0.52705
+ }
+ },
+ "backLeft":{
+ "x":{
+ "exp":"-20.75 in",
+ "val":-0.52705
+ },
+ "y":{
+ "exp":"20.75 in",
+ "val":0.52705
+ }
+ },
+ "mass":{
+ "exp":"125 lbs",
+ "val":56.69904625
+ },
+ "inertia":{
+ "exp":"7 kg m ^ 2",
+ "val":7.0
+ },
+ "gearing":{
+ "exp":"7.03125",
+ "val":7.03125
+ },
+ "radius":{
+ "exp":"2 in",
+ "val":0.0508
+ },
+ "vmax":{
+ "exp":"6000 RPM",
+ "val":628.3185307179587
+ },
+ "tmax":{
+ "exp":"7 N * m",
+ "val":7.0
+ },
+ "cof":{
+ "exp":"1",
+ "val":1.0
+ },
+ "bumper":{
+ "front":{
+ "exp":"16 in",
+ "val":0.4064
+ },
+ "side":{
+ "exp":"16 in",
+ "val":0.4064
+ },
+ "back":{
+ "exp":"16 in",
+ "val":0.4064
+ }
+ },
+ "differentialTrackWidth":{
+ "exp":"22 in",
+ "val":0.5588
+ }
+ },
+ "generationFeatures":[],
+ "codegen":{
+ "root":"home/moo/Projects/robotics/FRC2026/src/main/java/choreo",
+ "genVars":true,
+ "genTrajData":true,
+ "useChoreoLib":true
+ }
+}
--- /dev/null
+{
+ "version": 1.0,
+ "grid_size": 128,
+ "tabs": [
+ {
+ "name": "Teleoperated",
+ "grid_layout": {
+ "layouts": [],
+ "containers": [
+ {
+ "title": "Autos Result",
+ "x": 1408.0,
+ "y": 512.0,
+ "width": 256.0,
+ "height": 128.0,
+ "type": "Large Text Display",
+ "properties": {
+ "topic": "/SmartDashboard/WON AUTO?",
+ "period": 0.06,
+ "data_type": "string"
+ }
+ },
+ {
+ "title": "Field",
+ "x": 0.0,
+ "y": 0.0,
+ "width": 512.0,
+ "height": 512.0,
+ "type": "Field",
+ "properties": {
+ "topic": "/SmartDashboard/Field",
+ "period": 0.06,
+ "field_game": "Rebuilt",
+ "robot_width": 0.85,
+ "robot_length": 0.85,
+ "show_other_objects": true,
+ "show_trajectories": true,
+ "field_rotation": 0.0,
+ "robot_color": 4294198070,
+ "trajectory_color": 4294967295,
+ "show_robot_outside_widget": true
+ }
+ },
+ {
+ "title": "Voltage",
+ "x": 0.0,
+ "y": 512.0,
+ "width": 512.0,
+ "height": 128.0,
+ "type": "Voltage View",
+ "properties": {
+ "topic": "/AdvantageKit/PowerDistribution/Voltage",
+ "period": 0.06,
+ "data_type": "double",
+ "min_value": 4.0,
+ "max_value": 13.0,
+ "divisions": 10,
+ "inverted": false,
+ "orientation": "horizontal"
+ }
+ },
+ {
+ "title": "RobotID",
+ "x": 0.0,
+ "y": 768.0,
+ "width": 256.0,
+ "height": 128.0,
+ "type": "Large Text Display",
+ "properties": {
+ "topic": "/SmartDashboard/RobotID",
+ "period": 0.06,
+ "data_type": "string"
+ }
+ },
+ {
+ "title": "Time till Unactive",
+ "x": 1280.0,
+ "y": 0.0,
+ "width": 384.0,
+ "height": 256.0,
+ "type": "Radial Gauge",
+ "properties": {
+ "topic": "/SmartDashboard/Time till Unactive",
+ "period": 0.06,
+ "data_type": "double",
+ "start_angle": -120.0,
+ "end_angle": 120.0,
+ "min_value": 0.0,
+ "max_value": 100.0,
+ "number_of_labels": 6,
+ "wrap_value": false,
+ "show_pointer": true,
+ "show_ticks": true
+ }
+ },
+ {
+ "title": "Time till active",
+ "x": 1280.0,
+ "y": 256.0,
+ "width": 384.0,
+ "height": 256.0,
+ "type": "Radial Gauge",
+ "properties": {
+ "topic": "/SmartDashboard/Time till active",
+ "period": 0.06,
+ "data_type": "double",
+ "start_angle": -120.0,
+ "end_angle": 120.0,
+ "min_value": 0.0,
+ "max_value": 30.0,
+ "number_of_labels": 6,
+ "wrap_value": false,
+ "show_pointer": true,
+ "show_ticks": false
+ }
+ },
+ {
+ "title": "Hub Active",
+ "x": 1280.0,
+ "y": 512.0,
+ "width": 128.0,
+ "height": 128.0,
+ "type": "Boolean Box",
+ "properties": {
+ "topic": "/SmartDashboard/Hub Active",
+ "period": 0.06,
+ "data_type": "boolean",
+ "true_color": 4283215696,
+ "false_color": 4294198070,
+ "true_icon": "None",
+ "false_icon": "None"
+ }
+ },
+ {
+ "title": "Auto chooser",
+ "x": 0.0,
+ "y": 640.0,
+ "width": 512.0,
+ "height": 128.0,
+ "type": "ComboBox Chooser",
+ "properties": {
+ "topic": "/SmartDashboard/Auto chooser",
+ "period": 0.06,
+ "sort_options": true
+ }
+ },
+ {
+ "title": "Spindexer Jamming",
+ "x": 1280.0,
+ "y": 768.0,
+ "width": 384.0,
+ "height": 128.0,
+ "type": "Boolean Box",
+ "properties": {
+ "topic": "/SmartDashboard/Spindexer Jamming",
+ "period": 0.06,
+ "data_type": "boolean",
+ "true_color": 4283215696,
+ "false_color": 4294198070,
+ "true_icon": "None",
+ "false_icon": "None"
+ }
+ }
+ ]
+ }
+ },
+ {
+ "name": "Autonomous",
+ "grid_layout": {
+ "layouts": [],
+ "containers": []
+ }
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{"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
--- /dev/null
+{
+ "robotWidth": 0.832,
+ "robotLength": 0.832,
+ "holonomicMode": true,
+ "pathFolders": [
+ "Center Autos",
+ "Field Testing Autos",
+ "Left Autos",
+ "Miscellaneous",
+ "week 5 (new stuff)",
+ "Right Autos",
+ "week 2 autos"
+ ],
+ "autoFolders": [
+ "Week 5 autos",
+ "Testing",
+ "Week 2 autos"
+ ],
+ "defaultMaxVel": 3.0,
+ "defaultMaxAccel": 2.5,
+ "defaultMaxAngVel": 200.0,
+ "defaultMaxAngAccel": 300.0,
+ "defaultNominalVoltage": 12.0,
+ "robotMass": 63.37,
+ "robotMOI": 6.6,
+ "robotTrackwidth": 0.546,
+ "driveWheelRadius": 0.05,
+ "driveGearing": 7.03,
+ "maxDriveSpeed": 5.41,
+ "driveMotorType": "krakenX60",
+ "driveCurrentLimit": 52.0,
+ "wheelCOF": 0.9,
+ "flModuleX": 0.2635,
+ "flModuleY": 0.2635,
+ "frModuleX": 0.2635,
+ "frModuleY": -0.2635,
+ "blModuleX": -0.2635,
+ "blModuleY": 0.2635,
+ "brModuleX": -0.2635,
+ "brModuleY": -0.2635,
+ "bumperOffsetX": 0.0,
+ "bumperOffsetY": 0.0,
+ "robotFeatures": [
+ "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.5967,\"length\":0.254},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
+ ]
+}
\ No newline at end of file
--- /dev/null
+// 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 first;
+
+import org.wpilib.framework.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+ private Main() {}
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(frc.robot.Robot.class);
+ }
+}
--- /dev/null
+// 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 java.util.Optional;
+
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import au.grapplerobotics.CanBridge;
+import org.wpilib.net.PortForwarder;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.system.RobotController;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.CommandScheduler;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.BuildData;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends LoggedRobot {
+ private Command autoCommand;
+ private RobotContainer robotContainer;
+
+ public Robot() {
+ CanBridge.runTCP();
+ PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800);
+ PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182);
+
+ // Set up data receivers & replay source
+ switch (Constants.CURRENT_MODE) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case SIM:
+ // Running a physics simulator, log to NT
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case REPLAY:
+ // Replaying a log, set up replay source
+ setUseTiming(false); // Run as fast as possible
+ String logPath = LogFileUtil.findReplayLog();
+ Logger.setReplaySource(new WPILOGReader(logPath));
+ Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ break;
+ }
+ Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
+ // be added.
+ }
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // To Set the Robot Identity
+ // SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
+ // changes networktables.json, networktables.json.bck (both Untracked)
+ // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
+ // RobotId.setRobotId(RobotId.SwerveCompetition);
+
+ RobotController.setBrownoutVoltage(4.6); // TODO might break on systemcores
+ // https://www.chiefdelphi.com/t/frc-1678-citrus-circuits-systemcore-alpha-testing-thread/506842
+ // obtain this robot's identity
+ RobotId robotId = RobotId.getRobotId();
+
+ DriveConstants.update(robotId);
+
+ // Record metadata
+ Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildData.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildData.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH);
+ switch (BuildData.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncomitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
+ }
+
+ robotContainer = new RobotContainer(robotId);
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode-specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+
+ CommandScheduler.getInstance().run();
+
+ robotContainer.logComponents();
+ robotContainer.periodic();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically when the robot is disabled */
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ // Disable vision if the constant is false.
+ robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO);
+
+ // Get the autonomous command.
+ // This access is fast (about 14 microseconds) because the value is already resident in the
+ // Network Tables.
+ // There was a problem last year because the operation also installed about over a dozen items
+ // (taking more than 20 ms).
+ autoCommand = robotContainer.getAutoCommand();
+
+ // If there is an autonomous command, then schedule it
+ if (autoCommand != null) {
+ CommandScheduler.getInstance().schedule(autoCommand);
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ /** This function is called once each time the robot enters Teleop mode. */
+ @Override
+ public void teleopInit() {
+ robotContainer.setVisionEnabled(true);
+
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (autoCommand != null) {
+ autoCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ /** This function is called once each time the robot enters Test mode. */
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+
+ @Override
+ public void simulationPeriodic() {}
+
+ /**
+ * Gets the set Alliance; defaults to red if not set. This method replaces {@link
+ * edu.first.wpilibj.DriverStation.getAlliance}. The .get() is not necessary, so
+ * DriverStation.getAlliance().get() becomes Robot.getAlliance()
+ */
+ public static Alliance getAlliance() {
+ Optional<Alliance> dsAlliance = DriverStation.getAlliance();
+ if (dsAlliance.isPresent()) return dsAlliance.get();
+ else return Alliance.Red; // default to Red alliance
+ }
+}
--- /dev/null
+package frc.robot;
+
+import java.util.function.BooleanSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.AutoBuilderException;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+import choreo.auto.AutoChooser;
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.system.RobotController;
+import org.wpilib.livewindow.LiveWindow;
+import org.wpilib.smartdashboard.SendableChooser;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.CommandScheduler;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder;
+import frc.robot.commands.auto_comm.DynamicAutoBuilder;
+import frc.robot.commands.drive_comm.SysIDDriveCommand;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.controls.Operator;
+import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.util.PathGroupLoader;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and trigger mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems are defined here...
+ private Drivetrain drive = null;
+ private Vision vision = null;
+ // private LED led = null;
+
+ // Controllers are defined here
+ private BaseDriverConfig driver = null;
+ private Operator operator = null;
+
+ private EMABreaker breaker = null;
+
+ // auto Command selection
+ private final SendableChooser<Command> autoChooser = new SendableChooser<>();
+ private final AutoChooser choreoAutoChooser = new AutoChooser();
+
+ // choreo auto factory
+ AutoFactory autoFactory;
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ *
+ * <p>Different robots may have different subsystems.
+ */
+ public RobotContainer(RobotId robotId) {
+ // display the current robot id on smartdashboard
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putString("RobotID", robotId.toString());
+
+ SmartDashboard.putNumber("Match Time", 0.0);
+ }
+
+ // Filling the SendableChooser on SmartDashboard
+
+ // dispatch on the robot
+ switch (robotId) {
+ case TestBed1:
+ break;
+
+ case TestBed2:
+ break;
+
+ default:
+
+ case TwinBot:
+
+ case PrimeJr: // AKA Valence
+
+ case WaffleHouse: // AKA Betabot
+
+ case SwerveCompetition: // AKA "Vantage"
+
+ case BetaBot: // AKA "Pancake"
+ vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
+ // fall-through
+
+ case Vivace:
+
+ case Phil: // AKA "IHOP"
+
+ case Vertigo: // AKA "French Toast"
+ drive = new Drivetrain(vision, new GyroIOPigeon2());
+ driver = new PS5ControllerDriverConfig(drive);
+ operator = new Operator(drive);
+
+ initChoreo();
+
+ // Detected objects need access to the drivetrain
+ DetectedObject.setDrive(drive);
+
+ // SignalLogger.start();
+ driver.configureControls();
+ operator.configureControls();
+
+ registerCommands();
+ PathGroupLoader.loadPathGroups();
+
+ initializeAutoBuilder();
+ autoChooserInit();
+
+ if (drive != null && driver != null) {
+ // drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ SmartDashboard.putData("SysId Characterization", new SysIDDriveCommand(drive));
+ }
+ break;
+ }
+
+ // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more
+ // crt for this)
+ // This is really annoying so it's disabled
+ DriverStation.silenceJoystickConnectionWarning(true);
+
+ CommandScheduler.getInstance().schedule(new LogCommand());
+
+ // TODO: verify this claim.
+ // LiveWindow is causing periodic loop overruns
+ LiveWindow.disableAllTelemetry();
+ LiveWindow.setEnabled(false);
+ }
+
+ private void initChoreo() {
+ // choreo auto factory init
+ autoFactory =
+ new AutoFactory(
+ drive::getPose,
+ drive::resetOdometry,
+ sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+ true,
+ drive,
+ (trajectory, startOrFinish) -> {
+ Logger.recordOutput("Autos/Trajectory", trajectory.getPoses());
+ Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish);
+ });
+
+ // warmup command for choreo, prevents lag on auto startup
+ CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true));
+ }
+
+ /** Sets whether the drivetrain uses vision toupdate odometry */
+ public void setVisionEnabled(boolean enabled) {
+ if (drive != null) drive.setVisionEnabled(enabled);
+ }
+
+ public void initializeAutoBuilder() {
+ AutoBuilder.configure(
+ () -> drive.getPose(),
+ (pose) -> {
+ drive.resetOdometry(pose);
+ },
+ () -> drive.getChassisSpeeds(),
+ (chassisSpeeds) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Auto/ChassisSpeeds", chassisSpeeds);
+ }
+ drive.setChassisSpeeds(chassisSpeeds, false); // problem??
+ },
+ AutoConstants.AUTO_CONTROLLER,
+ AutoConstants.CONFIG,
+ getAllianceColorBooleanSupplier(),
+ drive);
+ }
+
+ public void registerCommands() {}
+
+ public void addAuto(String name) {
+ try {
+ Command auto = new PathPlannerAuto(name);
+ autoChooser.addOption(name, auto);
+ }
+ // is this the right one??
+ catch (AutoBuilderException e) {
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
+ public void addAuto(String name, Command auto) {
+ try {
+ autoChooser.addOption(name, auto);
+ } catch (AutoBuilderException e) {
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
+ public void addChoreoAuto(String name, AutoRoutine auto) {
+ choreoAutoChooser.addCmd(name, auto::cmd);
+ }
+
+ /**
+ * Initialize the SendableChooser on the SmartDashboard. Fill the SendableChooser with available
+ * Commands.
+ */
+ public void autoChooserInit() {
+ // add the options to the Chooser
+
+ autoChooser.setDefaultOption("Default", getDefaultAuto());
+
+ DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder();
+
+ // names
+ String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe";
+ String rightDynamicLiberalDoubleSwipe = "RightDynamicDoubleLiberalSwipe";
+ String leftDynamicConservativeDoubleSwipe = "LeftDynamicDoubleConservativeSwipe";
+ String rightDynamicConservativeDoubleSwipe = "RightDynamicDoubleConservativeSwipe";
+ // String leftDynamicShallowDoubleSwipe = "LeftDynamicShallowDoubleSwipe";
+ // String rightDynamicShallowDoubleSwipe = "RightDynamicShallowDoubleSwipe";
+
+ ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder();
+
+ // addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true,
+ // autoFactory));
+
+ // put the Chooser on the SmartDashboard
+ SmartDashboard.putData("Auto chooser", autoChooser);
+ SmartDashboard.putData("Choreo auto chooser", choreoAutoChooser);
+ }
+
+ public static BooleanSupplier getAllianceColorBooleanSupplier() {
+ return () -> {
+ // Boolean supplier that controls when the path will be mirrored for the red
+ // alliance
+ // This will flip the path being followed to the red side of the field.
+ // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isPresent()) {
+ return alliance.get() == DriverStation.Alliance.Red;
+ }
+ return false;
+ };
+ }
+
+ public boolean brownout() {
+ if (RobotController.getBatteryVoltage() < 6.0) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ public Command getDefaultAuto() {
+ return new DoNothing();
+ }
+
+ public Command getAutoCommand() {
+ // return autoChooser.getSelected();
+ return choreoAutoChooser.selectedCommand();
+ }
+
+ public void logComponents() {
+ if (!Constants.LOG_MECHANISMS) return;
+
+ Logger.recordOutput(
+ "ComponentPoses",
+ new Pose3d[] {
+ // Subsystem Pose3ds
+ });
+ }
+
+ public void periodic() {}
+}
--- /dev/null
+package frc.robot;
+
+import org.wpilib.util.Preferences;
+
+/**
+ * Set of known Robot Names.
+ *
+ * <p>The name of a robot in the RoboRIO's persistent memory. At deploy time, that name is used to
+ * set the corresponding RobotId.
+ *
+ * <p>Note that the RobotId is determined at Deploy time.
+ */
+public enum RobotId {
+ Default,
+ PrimeJr,
+ WaffleHouse,
+ TwinBot,
+ SwerveCompetition,
+ Vertigo,
+ Vivace,
+ Phil,
+ BetaBot,
+ ClassBot1,
+ ClassBot2,
+ ClassBot3,
+ ClassBot4,
+ TestBed1,
+ TestBed2;
+
+ /** The key used to access the RobotId name in the RoboRIO's persistent memory. */
+ public static final String ROBOT_ID_KEY = "RobotId";
+
+ /**
+ * Is this robot a classbot?
+ *
+ * @return true if a classbot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isClassBot() {
+ return this == WaffleHouse
+ || this == ClassBot1
+ || this == ClassBot2
+ || this == ClassBot3
+ || this == ClassBot4;
+ }
+
+ /**
+ * Whether this robot is a swerve bot
+ *
+ * @return true if a swerve bot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isSwerveBot() {
+ return this == SwerveCompetition
+ || this == Phil
+ || this == Vertigo
+ || this == Vivace
+ || this == BetaBot;
+ }
+
+ /**
+ * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory).
+ *
+ * @returns the RobotId
+ */
+ public static RobotId getRobotId() {
+ // assume a default identity
+ RobotId robotId = RobotId.Default;
+
+ // check whether Preferences has an entry for the RobotId
+ if (!Preferences.containsKey(ROBOT_ID_KEY)) {
+ // There is no such key. Set it to the default identity.
+ // This step guarantees persistent memory will have a key.
+ setRobotId(RobotId.Default);
+ }
+
+ // Remove the "Default" key if present.
+ // This key was the result of a programming error in 2023.
+ if (Preferences.containsKey("Default")) {
+ Preferences.remove("Default");
+ }
+
+ // get the RobotId string from the RoboRIO's Preferences
+ String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name());
+
+ // match that string to a RobotId by looking at all possible RobotId enums
+ for (RobotId rid : RobotId.values()) {
+ // does the preference string match the RobotId enum?
+ if (strId.equals(rid.name())) {
+ // yes, this instance is the desired RobotId
+ robotId = rid;
+ break;
+ }
+ }
+
+ if (robotId == RobotId.Default) {
+ if (Robot.isSimulation()) {
+ robotId = RobotId.SwerveCompetition; // Default to competition robot for simulation
+ } else {
+ throw new RuntimeException(
+ "RobotId is set to Default (or was unset)! Please set it to something.");
+ }
+ }
+
+ // return the robot identity
+ return robotId;
+ }
+
+ /**
+ * Set the RobotId in the RoboRIO's preferences (flash memory).
+ *
+ * <p>Calling it after the robot has been constructed (robotInit()) does not affect the robot.
+ */
+ static void setRobotId(RobotId robotId) {
+ // Set the robot identity in the RoboRIO Preferences
+ Preferences.setString(ROBOT_ID_KEY, robotId.name());
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import org.wpilib.command2.InstantCommand;
+
+/** Does nothing. Can be used to more clearly mark commands intended not to do anything. */
+public class DoNothing extends InstantCommand {}
--- /dev/null
+package frc.robot.commands;
+
+import org.wpilib.command2.Command;
+import frc.robot.constants.Constants;
+
+/// Command for logging stuff
+public class LogCommand extends Command {
+
+ private boolean hubActive = false;
+
+ public LogCommand() {}
+
+ @Override
+ public void execute() {
+ if (Constants.DISABLE_LOGGING) {
+ return;
+ }
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import org.wpilib.system.Filesystem;
+import org.wpilib.command2.Command;
+
+public class Music extends Command {
+ private Orchestra orchestra;
+
+ public Music(TalonFX[] motors) {
+ orchestra = new Orchestra(Filesystem.getDeployDirectory() + "/chirp/file.chrp");
+ for (TalonFX motor : motors) {
+ System.out.println(motor.getDescription());
+ orchestra.addInstrument(motor);
+ }
+ }
+
+ @Override
+ public void initialize() {
+ orchestra.play();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ orchestra.stop();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Subsystem;
+
+import java.util.function.Supplier;
+
+/**
+ * Runs the given command when this command is initialized, and ends when it ends. Useful for
+ * commands that are not created yet because the constructor parameters are not available until
+ * initialization. This is very similar to WPILib's DeferredCommand
+ */
+public class SupplierCommand extends Command {
+
+ private final Supplier<Command> commandSupplier;
+ private Command command;
+
+ /**
+ * Runs the given command when this command is initialized, and ends when it ends. Useful for
+ * commands that are not created yet because the constructor parameters are not available until
+ * initialization.
+ *
+ * @param commandSupplier A Supplier to the command to run
+ * @param Subsystem all subsystems that may be required to run the command supplied
+ */
+ public SupplierCommand(Supplier<Command> commandSupplier, Subsystem... Subsystem) {
+ addRequirements(Subsystem);
+ this.commandSupplier = commandSupplier;
+ }
+
+ @Override
+ public final void initialize() {
+ command = commandSupplier.get();
+ command.initialize();
+ }
+
+ @Override
+ public final void execute() {
+ command.execute();
+ }
+
+ @Override
+ public final void end(boolean interrupted) {
+ command.end(interrupted);
+ }
+
+ @Override
+ public final boolean isFinished() {
+ return command.isFinished();
+ }
+}
--- /dev/null
+package frc.robot.commands.auto_comm;
+
+import choreo.auto.AutoFactory;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.command2.InstantCommand;
+import frc.robot.commands.DoNothing;
+
+public class ChoreoPathCommandBuilder {
+
+ public ChoreoPathCommandBuilder() {}
+
+ public static Command basicTrajectoryAuto(
+ String pathName, boolean resetOdemetry, AutoFactory factory) {
+ Command command = factory.trajectoryCmd(pathName);
+
+ return Commands.sequence(
+ resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
+ command);
+ }
+}
--- /dev/null
+package frc.robot.commands.auto_comm;
+
+import org.wpilib.command2.*;
+
+public class DynamicAutoBuilder {
+
+ public DynamicAutoBuilder() {}
+
+ /*
+ * Autos have no named commands within them. They must be added here
+ * Still need to make one method to call that four command block in each
+ * sequential
+ */
+
+ // template:
+ // public Command templateAuto() {
+ // return new SequentialCommandGroup(
+ // new DoNothing()
+ // );
+ // }
+}
--- /dev/null
+// 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.commands.auto_comm;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.command2.InstantCommand;
+import org.wpilib.command2.SequentialCommandGroup;
+import frc.robot.RobotContainer;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.PathGroupLoader;
+
+/** Add your docs here. */
+public class FollowPathCommand extends SequentialCommandGroup {
+ Drivetrain drive;
+ PathPlannerPath path;
+
+ public FollowPathCommand(String name, Drivetrain drive) {
+ this(name, false, drive);
+ }
+
+ public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive) {
+ this.drive = drive;
+ this.path = PathGroupLoader.getPathGroup(pathName);
+ addCommands(
+ new InstantCommand(() -> resetOdemetry(resetOdemetry)),
+ new SupplierCommand(() -> AutoBuilder.followPath(path), drive) // "problem" (254)
+ // or pp's interaction with the drivetrain
+ // or pp config
+ );
+ }
+
+ public void resetOdemetry(boolean resetOdemetry) {
+ if (resetOdemetry) {
+ if (RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()) {
+ drive.resetOdometry(
+ new Pose2d(
+ path.getAllPathPoints().get(0).flip().position,
+ path.getIdealStartingState().flip().rotation()));
+ } else {
+ drive.resetOdometry(
+ new Pose2d(
+ path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation()));
+ }
+ }
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.command2.Command;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DriverAssist;
+
+/** Default drive command. Drives robot using driver controls. */
+public class DefaultDriveCommand extends Command {
+ protected final Drivetrain swerve;
+ private final BaseDriverConfig driver;
+ private PIDController trenchAssistPid = new PIDController(9, 0.0, 3);
+
+ public DefaultDriveCommand(Drivetrain swerve, BaseDriverConfig driver) {
+ this.swerve = swerve;
+ this.driver = driver;
+
+ addRequirements(swerve);
+ }
+
+ @Override
+ public void initialize() {
+ swerve.setStateDeadband(true);
+
+ trenchAssistPid.setIZone(2);
+ trenchAssistPid.setIntegratorRange(-1, 1);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("0 degrees snap location", 0);
+ }
+ }
+
+ @Override
+ public void execute() {
+ double forwardTranslation = driver.getForwardTranslation();
+ double sideTranslation = driver.getSideTranslation();
+ double rotation = -driver.getRotation();
+
+ double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
+
+ forwardTranslation *= slowFactor;
+ sideTranslation *= slowFactor;
+ rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
+
+ int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
+ forwardTranslation *= allianceReversal;
+ sideTranslation *= allianceReversal;
+
+ ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
+ ChassisSpeeds corrected =
+ DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+ }
+
+ /**
+ * Drives the robot
+ *
+ * @param speeds The ChassisSpeeds to drive at
+ */
+ protected void drive(ChassisSpeeds speeds) {
+ // If the driver is pressing the align button or a command set the drivetrain to
+ // align, then align to speaker
+ if (driver.getIsAlign() || swerve.getIsAlign()) {
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, swerve.getAlignAngle(), true);
+ } else {
+ swerve.drive(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ speeds.omegaRadiansPerSecond,
+ true,
+ false);
+ }
+ }
+}
--- /dev/null
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.commands.drive_comm;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.controller.ProfiledPIDController;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.trajectory.TrapezoidProfile;
+import org.wpilib.math.util.Units;
+import org.wpilib.command2.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.GeomUtil;
+
+public class DriveToPose extends Command {
+ protected static boolean updateTarget = false;
+ private static final double drivekP = 5.0;
+ private static final double drivekD = 0.0;
+ private static final double thetakP = 7.0;
+ private static final double thetakD = 0.0;
+ private static final double driveMaxVelocity = DriveConstants.MAX_SPEED;
+ private static final double driveMaxAcceleration = 2.6;
+ private static final double thetaMaxVelocity = 5.0;
+ private static final double thetaMaxAcceleration = 5.0;
+ private static final double driveTolerance = 0.015;
+ private static final double thetaTolerance = Units.degreesToRadians(1.0);
+ private static final double ffMinRadius = 0.05;
+ private static final double ffMaxRadius = 0.1;
+
+ private final Drivetrain drive;
+ private final Supplier<Pose2d> target;
+ private Pose2d targetPose;
+
+ private final ProfiledPIDController driveController =
+ new ProfiledPIDController(
+ drivekP,
+ 0.0,
+ drivekD,
+ new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration),
+ Constants.LOOP_TIME);
+ private final ProfiledPIDController thetaController =
+ new ProfiledPIDController(
+ thetakP,
+ 0.0,
+ thetakD,
+ new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration),
+ Constants.LOOP_TIME);
+
+ private Translation2d lastSetpointTranslation = new Translation2d();
+ private double driveErrorAbs = 0.0;
+ private double thetaErrorAbs = 0.0;
+ private boolean running = false;
+ private Supplier<Pose2d> robot;
+
+ private Supplier<Translation2d> linearFF = () -> Translation2d.kZero;
+ private DoubleSupplier omegaFF = () -> 0.0;
+
+ private Debouncer debouncer = new Debouncer(0.2);
+
+ public DriveToPose(Drivetrain drive, Supplier<Pose2d> target) {
+ this.drive = drive;
+ this.target = target;
+ robot = drive::getPose;
+
+ // Set tolerance
+ driveController.setTolerance(driveTolerance);
+ thetaController.setTolerance(thetaTolerance);
+
+ // Enable continuous input for theta controller
+ thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+ addRequirements(drive);
+ }
+
+ public DriveToPose(
+ Drivetrain drive,
+ Supplier<Pose2d> target,
+ Supplier<Translation2d> linearFF,
+ DoubleSupplier omegaFF) {
+ this(drive, target);
+ this.linearFF = linearFF;
+ this.omegaFF = omegaFF;
+ }
+
+ @Override
+ public void initialize() {
+ drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+
+ targetPose = target.get();
+ Pose2d currentPose = robot.get();
+ ChassisSpeeds fieldVelocity =
+ ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
+ Translation2d linearFieldVelocity =
+ new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
+
+ thetaController.reset(
+ currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond);
+ lastSetpointTranslation = currentPose.getTranslation();
+
+ if (targetPose != null) {
+ driveController.reset(
+ currentPose.getTranslation().getDistance(target.get().getTranslation()),
+ -linearFieldVelocity
+ .rotateBy(
+ targetPose
+ .getTranslation()
+ .minus(currentPose.getTranslation())
+ .getAngle()
+ .unaryMinus())
+ .getX());
+ }
+ }
+
+ @Override
+ public void execute() {
+ running = true;
+
+ // Get current pose and target pose
+ Pose2d currentPose = robot.get();
+ if (updateTarget) {
+ targetPose = target.get();
+ }
+ if (targetPose == null) {
+ return;
+ }
+
+ // Calculate drive speed
+ double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
+ double ffScaler =
+ MathUtil.clamp((currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), 0.0, 1.0);
+ driveErrorAbs = currentDistance;
+ driveController.reset(
+ lastSetpointTranslation.getDistance(targetPose.getTranslation()),
+ driveController.getSetpoint().velocity);
+ double driveVelocityScalar =
+ driveController.getSetpoint().velocity * ffScaler
+ + driveController.calculate(driveErrorAbs, 0.0);
+ if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0;
+ lastSetpointTranslation =
+ new Pose2d(
+ targetPose.getTranslation(),
+ currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+ .transformBy(GeomUtil.toTransform2d(driveController.getSetpoint().position, 0.0))
+ .getTranslation();
+
+ // Calculate theta speed
+ double thetaVelocity =
+ thetaController.getSetpoint().velocity * ffScaler
+ + thetaController.calculate(
+ currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
+ thetaErrorAbs =
+ Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians());
+ if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0;
+
+ Translation2d driveVelocity =
+ new Pose2d(
+ new Translation2d(),
+ currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+ .transformBy(GeomUtil.toTransform2d(driveVelocityScalar, 0.0))
+ .getTranslation();
+
+ // Scale feedback velocities by input ff
+ final double linearS = linearFF.get().getNorm() * 3.0;
+ final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0;
+ driveVelocity =
+ driveVelocity.interpolate(linearFF.get().times(DriveConstants.MAX_SPEED), linearS);
+ thetaVelocity =
+ MathUtil.interpolate(
+ thetaVelocity, omegaFF.getAsDouble() * DriveConstants.MAX_ANGULAR_SPEED, thetaS);
+
+ // Command speeds
+ drive.drive(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, true, false);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ drive.setVisionEnabled(true);
+ running = false;
+ }
+
+ /** Checks if the robot is stopped at the final pose. */
+ public boolean atGoal() {
+ return running && (driveController.atGoal() && thetaController.atGoal() || targetPose == null);
+ }
+
+ /** Checks if the robot pose is within the allowed drive and theta tolerances. */
+ public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) {
+ return running
+ && (Math.abs(driveErrorAbs) < driveTolerance
+ && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians()
+ || targetPose == null);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return debouncer.calculate(withinTolerance(driveTolerance, new Rotation2d(thetaTolerance)));
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.InstantCommand;
+import org.wpilib.command2.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Moves the robot to a pose using PathPlanner */
+public class GoToPose extends SequentialCommandGroup {
+
+ private Drivetrain drive;
+ private Supplier<Pose2d> poseSupplier;
+ private double maxSpeed;
+ private double maxAccel;
+
+ /**
+ * Uses PathPlanner to go to a pose
+ *
+ * @param poseSupplier The supplier for the pose to use
+ * @param drive The drivetrain
+ */
+ public GoToPose(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
+ this(poseSupplier, AutoConstants.MAX_AUTO_SPEED, AutoConstants.MAX_AUTO_ACCEL, drive);
+ }
+
+ public GoToPose(Pose2d pose, Drivetrain drive) {
+ this(() -> pose, drive);
+ }
+
+ /**
+ * Uses PathPlanner to go to a pose
+ *
+ * @param poseSupplier The supplier for the pose to use
+ * @param maxSpeed The maximum speed to use
+ * @param maxAccel The maximum acceleration to use
+ * @param drive The drivetrain
+ */
+ public GoToPose(
+ Supplier<Pose2d> poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) {
+ this.poseSupplier = poseSupplier;
+ this.maxSpeed = maxSpeed;
+ this.maxAccel = maxAccel;
+ this.drive = drive;
+ addCommands(
+ new InstantCommand(() -> drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)),
+ new SupplierCommand(() -> createCommand(), drive)
+ .handleInterrupt(() -> drive.setVisionEnabled(true)),
+ new InstantCommand(() -> drive.setVisionEnabled(true)));
+ }
+
+ /** Creates the PathPlanner command and schedules it */
+ public Command createCommand() {
+ Pose2d pose = poseSupplier.get();
+ if (pose == null) {
+ return new DoNothing();
+ }
+ Command command =
+ AutoBuilder.pathfindToPose(
+ pose,
+ new PathConstraints(
+ maxSpeed,
+ maxAccel,
+ DriveConstants.MAX_ANGULAR_SPEED,
+ DriveConstants.MAX_ANGULAR_ACCEL),
+ 0);
+
+ // get the distance to the pose.
+ double dist = drive.getPose().minus(pose).getTranslation().getNorm();
+
+ // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner
+ // makes weird paths.
+ if (dist > 3) {
+ command = new DoNothing();
+ DriverStation.reportWarning("Alignment Path too long, doing nothing, GoToPose.java", false);
+ } else if (dist < 0.02) {
+ command = new DoNothing();
+ DriverStation.reportWarning("Alignment Path too short, doing nothing, GoToPose.java", false);
+ }
+
+ return command;
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.command2.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Runs the chassis PIDs to move the robot to a specific pose. */
+public class GoToPosePID extends Command {
+
+ private Drivetrain drive;
+
+ private Supplier<Pose2d> poseSupplier;
+ private Pose2d pose;
+
+ /**
+ * Runs the chassis PIDs to move the robot to a specific pose.
+ *
+ * @param pose The pose supplier to go to
+ * @param drive The drivetrain
+ */
+ public GoToPosePID(Supplier<Pose2d> pose, Drivetrain drive) {
+ this.drive = drive;
+ this.poseSupplier = pose;
+
+ addRequirements(drive);
+ }
+
+ public GoToPosePID(Pose2d pose, Drivetrain drive) {
+ this(() -> pose, drive);
+ }
+
+ @Override
+ public void initialize() {
+ pose = poseSupplier.get();
+ drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+ }
+
+ @Override
+ public void execute() {
+ if (pose == null) {
+ return;
+ }
+
+ drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians());
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ drive.setVisionEnabled(true);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return pose == null
+ || drive.getXController().atSetpoint()
+ && drive.getYController().atSetpoint()
+ && drive.getRotationController().atSetpoint();
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.util.Units;
+import org.wpilib.command2.InstantCommand;
+import org.wpilib.command2.RunCommand;
+import org.wpilib.command2.SequentialCommandGroup;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Sets the robot's wheels to an X formation to prevent being pushed around by other bots. */
+public class SetFormationX extends SequentialCommandGroup {
+ public SetFormationX(Drivetrain drive) {
+ addRequirements(drive);
+ addCommands(
+ new InstantCommand(() -> drive.setStateDeadband(false), drive),
+ new RunCommand(
+ () ->
+ drive.setModuleStates(
+ new SwerveModuleState[] {
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
+ },
+ false),
+ drive));
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.command2.InstantCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach
+ * the angle requested in a certain time.
+ */
+public class SimplePresetSteerAngles extends InstantCommand {
+
+ /**
+ * sets the angle of module steer to 0 to remove initial turn time and drift
+ *
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive) {
+ this(drive, new Rotation2d());
+ }
+
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param angle angle to set module steer to in radians
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, double angle) {
+ this(drive, new Rotation2d(angle));
+ }
+
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param rotation rotation to set module steer to
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) {
+ super(
+ () -> {
+ drive.setStateDeadband(false);
+ drive.setModuleStates(
+ new SwerveModuleState[] {
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation)
+ },
+ true);
+ drive.setStateDeadband(true);
+ },
+ drive);
+ drive.setStateDeadband(true);
+ }
+}
--- /dev/null
+// 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.commands.drive_comm;
+
+import org.littletonrobotics.junction.Logger;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.units.Units;
+import org.wpilib.command2.SequentialCommandGroup;
+import org.wpilib.command2.WaitCommand;
+import org.wpilib.command2.sysid.SysIdRoutine.Config;
+import org.wpilib.command2.sysid.SysIdRoutine.Direction;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SysId;
+
+/** A command to run all 4 SysId routines on the drivetrain */
+public class SysIDDriveCommand extends SequentialCommandGroup {
+
+ private Config config = new Config();
+ private SysId sysId;
+
+ public SysIDDriveCommand(Drivetrain drive) {
+ config =
+ new Config(
+ Units.Volts.of(0.5).per(Units.Seconds),
+ Units.Volts.of(3),
+ Units.Seconds.of(5),
+ (state) -> Logger.recordOutput("SysIdTestState", state.toString()));
+ Rotation2d[] angles = {
+ Rotation2d.fromDegrees(0), // -45-180
+ Rotation2d.fromDegrees(0), // 45
+ Rotation2d.fromDegrees(0), // 45+180
+ Rotation2d.fromDegrees(0), // -45
+ };
+ sysId =
+ new SysId(
+ "Drivetrain",
+ x -> {
+ drive.setAngleMotors(angles);
+ drive.setDriveVoltages(x);
+ },
+ drive,
+ config);
+ addCommands(
+ sysId.runQuasisStatic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runQuasisStatic(Direction.kReverse),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kReverse));
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.trajectory.Trajectory;
+import org.wpilib.math.trajectory.Trajectory.State;
+import org.wpilib.command2.InstantCommand;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Sets all module angles to a given trajectory's initial angle. */
+public class TrajectoryPresetSteerAngles extends InstantCommand {
+ /*
+ * make sure to add wait command after called to give time to correct
+ */
+ public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) {
+ super(
+ () -> {
+
+ // 0.01 is the time between trajectory samples, in seconds
+ // Can be replaced for any small number, but it should be the same as the time between all
+ // uses
+ double time = 0.01;
+
+ drive.setStateDeadband(false);
+
+ Pose2d initialPose = trajectory.getInitialPose();
+ State sample = trajectory.sample(time);
+ Pose2d nextPose = sample.poseMeters;
+
+ double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
+ double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
+ double angularVelo =
+ (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
+
+ ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
+ chassisSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
+
+ SwerveModuleState[] swerveModuleStates =
+ DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
+ for (SwerveModuleState swerveModuleState : swerveModuleStates) {
+ swerveModuleState.speedMetersPerSecond = 0;
+ }
+ drive.setModuleStates(swerveModuleStates, true);
+ drive.setStateDeadband(true);
+ },
+ drive);
+ }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.wpilib.command2.Command;
+import frc.robot.subsystems.PowerControl.Battery;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+
+public class PowerControl extends Command {
+ // my beautiful power control subsystems
+ private EMABreaker breaker;
+ private Battery battery;
+ // TODO: add subsystems back when implementing logic:
+ // the real subsystems
+ // private Drivetrain drivetrain;
+ // private Shooter shooter;
+ // private Turret turret;
+ // private Hood hood;
+ // private Intake intake;
+ // private Spindexer spindexer;
+
+ public SeverityLevel severityLevel;
+
+ public enum SeverityLevel {
+ SEVERITY_LVL_ZERO,
+ SEVERITY_LVL_ONE,
+ SEVERITY_LVL_TWO,
+ SEVERITY_LVL_THREE,
+ SEVERITY_LVL_FOUR,
+ SEVERITY_LVL_FIVE,
+ }
+
+ public PowerControl(
+ EMABreaker breaker, // pc
+ Battery battery // pc
+ // Drivetrain drivetrain, // main draw
+ // Shooter shooter, // aiming (vital)
+ // Turret turret, // aiming
+ // Hood hood, // aiming
+ // Intake intake, // bps
+ // Spindexer spindexer // bps
+ ) {
+ this.breaker = breaker;
+ this.battery = battery;
+ // this.drivetrain = drivetrain;
+ // this.shooter = shooter;
+ // this.turret = turret;
+ // this.hood = hood;
+ // this.intake = intake;
+ // this.spindexer = spindexer;
+
+ addRequirements(
+ breaker,
+ battery); // not sure if I'll need requirement access for setting new current limits
+ }
+
+ @Override
+ public void initialize() {
+ severityLevel = SeverityLevel.SEVERITY_LVL_ZERO;
+ }
+
+ @Override
+ public void execute() {
+ double[] worstFilter = breaker.percentageMaxUsage();
+ double percentage = worstFilter[0]; // percentage of current average until we trip breaker
+ double tau =
+ worstFilter[1]; // how quickly this issue is happenning and if we need to respond quickly
+
+ // Some logic here
+ }
+
+ @Override
+ public void end(boolean interupted) {
+ severityLevel =
+ SeverityLevel
+ .SEVERITY_LVL_ZERO; // in the case of disabling this command we shoud reset its effects
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.wpilib.command2.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AcquireGamePiece extends SequentialCommandGroup {
+ /**
+ * Intakes a game piece
+ *
+ * @param gamePiece The supplier for the game piece to intake
+ * @param drive The drivetrain
+ */
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive) {
+ // TODO: Replace DoNothing with next year's intake command
+ addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AimAtGamePiece extends DefaultDriveCommand {
+ private Supplier<DetectedObject> objectSupplier;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+ public AimAtGamePiece(
+ Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier) {
+ super(drive, driver);
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ @Override
+ protected void drive(ChassisSpeeds speeds) {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
+ super.drive(speeds);
+ return;
+ }
+ DetectedObject object = objectSupplier.get();
+
+ if (object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ super.drive(speeds);
+ return;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+
+ // System.out.println("objangle " + object.getAngle());
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ MathUtil.angleModulus(object.getAngle()),
+ true);
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import org.wpilib.vision.apriltag.AprilTag;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.util.Units;
+import org.wpilib.command2.Command;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Aims the robot at the closest April tag */
+public class AimAtTag extends Command {
+ private Drivetrain drive;
+ private PIDController pid;
+
+ /**
+ * Aims the robot at the closest April tag
+ *
+ * @param drive The drivetrain
+ */
+ public AimAtTag(Drivetrain drive) {
+ this.drive = drive;
+ // Copy drive PID and changetolerance
+ pid =
+ new PIDController(
+ drive.getRotationController().getP(),
+ drive.getRotationController().getI(),
+ drive.getRotationController().getD());
+ pid.setTolerance(Units.degreesToRadians(1));
+ addRequirements(drive);
+ }
+
+ /** Gets the closest tag and sets the setpoint to aim at it */
+ @Override
+ public void initialize() {
+ double dist = Double.POSITIVE_INFINITY;
+ Translation2d closest = new Translation2d();
+ Translation2d driveTranslation = drive.getPose().getTranslation();
+ for (AprilTag tag : FieldConstants.field.getTags()) {
+ Translation2d translation = tag.pose.toPose2d().getTranslation();
+ double dist2 = driveTranslation.getDistance(translation);
+ if (dist2 < dist) {
+ dist = dist2;
+ closest = translation;
+ }
+ }
+ pid.reset();
+ pid.setSetpoint(
+ Math.atan2(
+ closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
+ }
+
+ /** Runs the PID */
+ @Override
+ public void execute() {
+ double angle = drive.getPose().getRotation().getRadians();
+ // If the distance between the angles is more than 180 degrees, use an identical angle ±360
+ // degrees
+ if (angle - pid.getSetpoint() > Math.PI) {
+ angle -= 2 * Math.PI;
+ } else if (angle - pid.getSetpoint() < -Math.PI) {
+ angle += 2 * Math.PI;
+ }
+ double speed = pid.calculate(angle);
+ drive.drive(0, 0, speed, true, false);
+ }
+
+ /**
+ * Stops the drivetrain
+ *
+ * @param interrupted If the command is interrupted
+ */
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ }
+
+ /**
+ * Returns if the command is finished
+ *
+ * @return If the PID is at the setpoint
+ */
+ @Override
+ public boolean isFinished() {
+ return pid.atSetpoint();
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.ArrayList;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.system.Timer;
+import org.wpilib.command2.Command;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.MathUtils;
+import frc.robot.util.Vision.Vision;
+
+/** Calculates standard deviations for vision */
+public class CalculateStdDevs extends Command {
+ private final Vision vision;
+ private ArrayList<Pose2d> poses;
+ private int arrayLength;
+ private Timer endTimer;
+ private Drivetrain drive;
+
+ /**
+ * Constructor for CalculateStdDevs
+ *
+ * @param posesToUse the amount of poses to take the standard deviation of. More poses will take
+ * more time.
+ * @param vision The vision
+ */
+ public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) {
+ this.vision = vision;
+ this.drive = drive;
+ arrayLength = posesToUse;
+ endTimer = new Timer();
+ }
+
+ /** Resets the pose array */
+ @Override
+ public void initialize() {
+ // create the ArrayList of poses to store
+ // an ArrayList prevents issues if the command ends early, and makes checking if the command has
+ // finished easy
+ poses = new ArrayList<Pose2d>();
+
+ drive.setVisionEnabled(false);
+ }
+
+ /** Adds a pose to the array */
+ @Override
+ public void execute() {
+ Pose2d pose = vision.getPose2d(drive.getPose());
+ // If the pose exists, add it to the first open spot in the array
+ if (pose != null) {
+ // if we see a pose, reset the timer (it will be started the next time it doesn't get a pose)
+ endTimer.stop();
+ endTimer.reset();
+ // add the pose to our data
+ poses.add(pose);
+ if (poses.size() % 10 == 0) {
+ System.out.printf("%.0f%% done\n", ((double) poses.size()) / arrayLength * 100);
+ }
+ } else {
+ endTimer.start();
+ // If kStdDevCommandEndTime seconds have passed since it saw an April tag, stop the command
+ // Prevents it from running forever
+ if (endTimer.hasElapsed(10)) {
+ cancel();
+ }
+ }
+ }
+
+ /** Calculates the standard deviation */
+ @Override
+ public void end(boolean interrupted) {
+ drive.setVisionEnabled(true);
+
+ // If the array is empty, don't try to calculate std devs
+ if (poses.size() == 0) {
+ System.out.println(
+ "There are no poses in the array\nTry again where the robot can see an April tag.");
+ return;
+ }
+
+ // create arrays of the poses by X, Y, and Rotation for calculations
+ double[] xArray = new double[poses.size()];
+ double[] yArray = new double[poses.size()];
+ double[] rotArray = new double[poses.size()];
+
+ // copy the values into the arrays
+ for (int i = 0; i < poses.size(); i++) {
+ xArray[i] = poses.get(i).getX();
+ yArray[i] = poses.get(i).getY();
+ rotArray[i] = poses.get(i).getRotation().getRadians();
+ }
+
+ // Calculate the standard deviations
+ double stdDevX = MathUtils.stdDev(xArray);
+ double stdDevY = MathUtils.stdDev(yArray);
+ double stdDevRot = MathUtils.stdDev(rotArray);
+
+ // Find distance to tag
+ double distance;
+ try {
+ distance =
+ vision
+ .getEstimatedPoses(drive.getPose())
+ .get(0)
+ .targetsUsed
+ .get(0)
+ .getBestCameraToTarget()
+ .getTranslation()
+ .getNorm();
+ } catch (Exception e) {
+ System.out.println("Could not see a target");
+ distance = -1;
+ }
+
+ // Print and log values
+ System.out.printf(
+ "Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n",
+ stdDevX, stdDevY, stdDevRot, distance);
+ }
+
+ /**
+ * Returns if the command is finished
+ *
+ * @return If the array is full
+ */
+ @Override
+ public boolean isFinished() {
+ return poses.size() >= arrayLength;
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+/** Moves toward the detected object */
+public class DriveToGamePiece extends DriveToPose {
+ private static boolean constantUpdate = true;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+ /**
+ * Moves toward the detected object
+ *
+ * @param detectedObject The supplier for the detected object to use
+ * @param drive The drivetrain
+ */
+ public DriveToGamePiece(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
+ super(drive, () -> getPose(detectedObject, drive));
+ updateTarget = constantUpdate;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ public static Pose2d getPose(Supplier<DetectedObject> supplier, Drivetrain drive) {
+ DetectedObject object = supplier.get();
+ if (object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ return null;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+ Rotation2d rotation = new Rotation2d(MathUtil.angleModulus(object.getAngle()));
+ Translation2d objectTranslation = object.pose.toPose2d().getTranslation();
+ Translation2d diff = objectTranslation.minus(drive.getPose().getTranslation());
+ Translation2d translation =
+ objectTranslation.minus(
+ diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2 / diff.getNorm()));
+ return new Pose2d(translation, rotation);
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.command2.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class GoToPose2 extends Command {
+ private static final double MIN_ACCEL = 2;
+ private final Supplier<Pose2d> poseSupplier;
+ private final Drivetrain drive;
+ private Pose2d pose;
+ private double vx;
+ private double vy;
+ private Pose2d error;
+
+ public GoToPose2(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
+ this.poseSupplier = poseSupplier;
+ this.drive = drive;
+ addRequirements(drive);
+ }
+
+ @Override
+ public void initialize() {
+ pose = poseSupplier.get();
+ ChassisSpeeds v = drive.getChassisSpeeds();
+ vx = v.vxMetersPerSecond;
+ vy = v.vyMetersPerSecond;
+ error = null;
+ }
+
+ @Override
+ public void execute() {
+ if (pose == null) {
+ return;
+ }
+ Pose2d drivePose = drive.getPose();
+ error = drivePose.relativeTo(pose);
+ double ax = calcAccel(vx, error.getX());
+ double ay = calcAccel(vy, error.getY());
+ if (Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01) {
+ ax = -Math.signum(error.getX()) * MIN_ACCEL;
+ }
+ if (Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01) {
+ ay = -Math.signum(error.getY()) * MIN_ACCEL;
+ }
+ vx += ax * Constants.LOOP_TIME;
+ vy += ay * Constants.LOOP_TIME;
+ Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation());
+ drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return pose == null || error != null && error.getTranslation().getNorm() < 0.01;
+ }
+
+ private double calcAccel(double v, double x) {
+ if (Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5) {
+ return 0;
+ }
+ double a = v * v / 2 / x;
+ double a2 = -v / Constants.LOOP_TIME;
+ if (Math.abs(a2) < Math.abs(a)) {
+ return a2;
+ }
+ return a;
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import org.wpilib.command2.Command;
+import frc.robot.constants.Constants;
+import frc.robot.util.Vision.DetectedObject;
+
+public class LogVision extends Command {
+ private Supplier<DetectedObject> objectSupplier;
+
+ public LogVision(Supplier<DetectedObject> objectSupplier) {
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void execute() {
+ DetectedObject object = this.objectSupplier.get();
+ if (object != null) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Vision/object_angle", object.getAngle());
+ Logger.recordOutput("Vision/object_distance", object.getDistance());
+ }
+ }
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import org.wpilib.math.util.Units;
+import org.wpilib.system.Timer;
+import org.wpilib.command2.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/** Adds data from object detection vision to SmartDashboard */
+public class ReturnData extends Command {
+ private final Vision vision;
+ private final Timer timer = new Timer();
+
+ /**
+ * Adds data from object detection vision to Smartdashboard
+ *
+ * @param vision The vision
+ */
+ public ReturnData(Vision vision) {
+ this.vision = vision;
+ }
+
+ @Override
+ public void initialize() {
+ timer.reset();
+ timer.start();
+ }
+
+ /** Adds the data to SmartDashboard */
+ @Override
+ public void execute() {
+ if (timer.advanceIfElapsed(2)) {
+ double[] xOffset = vision.getHorizontalOffset();
+ double[] yOffset = vision.getVerticalOffset();
+ // long[] objectClass = vision.getDetectedObjectClass();
+
+ // //put the offsets and area on SmartDashboard for testing
+ // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset);
+ // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset);
+ // SmartDashboard.putNumberArray("Object Distances", vision.getDistance());
+
+ DetectedObject bestGamePiece = vision.getBestGamePiece(Math.PI, false);
+ if (bestGamePiece != null) {
+ // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
+ System.out.println("\n\nBest game piece: " + bestGamePiece);
+ }
+
+ if ((xOffset.length != 0) == (yOffset.length != 0)) {
+ for (int i = 0; i < xOffset.length; i++) {
+ System.out.printf(
+ "\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0));
+ DetectedObject object =
+ new DetectedObject(
+ Units.degreesToRadians(xOffset[i]),
+ Units.degreesToRadians(yOffset[i]),
+ 0,
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
+ System.out.printf(
+ "Object: %s\nDistance: %.2f, Angle: %.2f\n",
+ object, object.getDistance(), Units.radiansToDegrees(object.getAngle()));
+ }
+ } else {
+ System.out.println("One of the arrays is empty!");
+ }
+ }
+ }
+
+ /**
+ * Does nothing
+ *
+ * @param interrupted If the command is interrupted
+ */
+ @Override
+ public void end(boolean interrupted) {}
+
+ /**
+ * Returns if the command is finished
+ *
+ * @retrun Always false (command never finishes)
+ */
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import org.wpilib.command2.ParallelCommandGroup;
+import frc.robot.constants.VisionConstants;
+
+/** Shutdown all Orange Pis listed by hostname in {@link frc.robot.constants.VisionConstants} */
+public class ShutdownAllPis extends ParallelCommandGroup {
+ public ShutdownAllPis() {
+ ShutdownOrangePi[] commands = new ShutdownOrangePi[VisionConstants.ORANGEPI_HOSTNAMES.length];
+ for (int i = 0; i < commands.length; i++) {
+ commands[i] = new ShutdownOrangePi(VisionConstants.ORANGEPI_HOSTNAMES[i]);
+ }
+
+ addCommands(commands);
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardCopyOption;
+import java.nio.file.attribute.PosixFilePermissions;
+
+import org.wpilib.system.Filesystem;
+import org.wpilib.command2.Command;
+import frc.robot.Robot;
+import frc.robot.constants.VisionConstants;
+
+/**
+ * Run the ssh command to shutdown a single Orange Pi. Uses the username and password set in {@link
+ * frc.robot.constants.VisionConstants}.
+ */
+public class ShutdownOrangePi extends Command {
+ private String hostname;
+ private Process process;
+
+ /**
+ * @param hostname The hostname or IP of the orangepi to shut down.
+ */
+ public ShutdownOrangePi(String hostname) {
+ assert hostname != null;
+ this.hostname = hostname;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public void initialize() {
+ if (Robot.isSimulation()) {
+ // needs to run on an actual roborio because of architecture-specific binaries
+ System.out.println("Would shut down OrangePi at " + hostname + " if this was real.");
+ return;
+ }
+
+ try {
+ String initialPath = Filesystem.getDeployDirectory() + "/sshpass";
+ Path initalPathPath = Path.of(initialPath);
+ String binPath = "/home/lvuser/sshpass2";
+ Path binPathPath = Path.of(binPath);
+ // copies to be able to get executable permissions on the new binary
+ Files.copy(initalPathPath, binPathPath, StandardCopyOption.REPLACE_EXISTING);
+ Files.setPosixFilePermissions(binPathPath, PosixFilePermissions.fromString("rwxr-xr-x"));
+
+ String[] commandString =
+ new String[] {
+ binPath,
+ "-p",
+ "raspberry",
+ "ssh",
+ "-o",
+ "StrictHostKeyChecking=no",
+ VisionConstants.ORANGEPI_USERNAME + "@" + hostname,
+ "sudo",
+ "shutdown",
+ "now"
+ };
+
+ this.process = Runtime.getRuntime().exec(commandString);
+ } catch (Exception e) {
+ String message = e.getMessage() == null ? "unknown" : e.getMessage();
+ System.out.println("Failed to shutdown OrangePi. Reason: " + e.getClass() + " -- " + message);
+ }
+ }
+
+ @Override
+ public void execute() {
+ if (this.process == null) return;
+
+ try {
+ InputStream stdout = this.process.getInputStream();
+ InputStream stderr = this.process.getErrorStream();
+
+ int remainingStdoutBytes = stdout.available();
+ int remainingStderrBytes = stderr.available();
+
+ if (remainingStdoutBytes > 0) {
+ byte[] stdoutBytes = stdout.readNBytes(remainingStdoutBytes);
+ System.out.println("OPI: " + new String(stdoutBytes, StandardCharsets.UTF_8));
+ }
+
+ if (remainingStderrBytes > 0) {
+ byte[] stderrBytes = stderr.readNBytes(remainingStderrBytes);
+ System.err.println("OPI: " + new String(stderrBytes, StandardCharsets.UTF_8));
+ }
+ } catch (IOException e) {
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return this.process == null || !this.process.isAlive();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (this.process == null) return;
+
+ if (this.process.isAlive()) {
+ this.process.destroy(); // end the process if we've been interrupted
+ } else {
+ // only grab exit value if the process has had time to exit
+ int exitValue = this.process.exitValue();
+ if (exitValue != 0) // abnormal termination
+ System.out.println(
+ "OrangePi shutdown of " + hostname + " failed with exit code " + exitValue + ".");
+ else System.out.println("OrangePi shutdown of " + hostname + " succesful.");
+ }
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.system.Timer;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.command2.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.Vision;
+
+/** Gathers data on the distance limits of the camera used for vision. */
+public class TestVisionDistance extends Command {
+ private final Drivetrain drive;
+ private final Vision vision;
+ private Translation2d visionStartTranslation, driveStartTranslation;
+ private Pose2d currentPose = null;
+ private double driveDistance;
+ private double visionDistance;
+
+ private double speed;
+
+ private final Timer endTimer = new Timer();
+ private final Timer printTimer = new Timer();
+
+ // How many seconds of not seeing april tag before ending the command
+ private static final double END_DELAY = 0.25;
+
+ // How many seconds between each data print
+ private static final double PRINT_DELAY = 1;
+
+ /**
+ * Constructor for TestVisionDistance
+ *
+ * @param speed What speed to move at, negative if backward
+ * @param drive The drivetrain
+ * @param vision The vision
+ */
+ public TestVisionDistance(double speed, Drivetrain drive, Vision vision) {
+ addRequirements(drive);
+ this.drive = drive;
+ this.speed = speed;
+ this.vision = vision;
+ }
+
+ /** Starts the timers and disables vision for odometry */
+ @Override
+ public void initialize() {
+
+ endTimer.reset();
+ printTimer.restart();
+
+ drive.setVisionEnabled(false);
+
+ currentPose = vision.getPose2d(drive.getPose());
+ visionStartTranslation = currentPose.getTranslation();
+ driveStartTranslation = drive.getPose().getTranslation();
+ driveDistance = 0;
+ visionDistance = 0;
+ }
+
+ /**
+ * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the
+ * distances
+ */
+ @Override
+ public void execute() {
+ drive.drive(speed, 0, 0, false, false);
+ Pose2d newestPose = vision.getPose2d(currentPose, drive.getPose());
+
+ // If the camera can see the apriltag
+ if (newestPose != null) {
+ // update current pose
+ currentPose = newestPose;
+ // reset the timer
+ endTimer.reset();
+ driveDistance = drive.getPose().getTranslation().getDistance(driveStartTranslation);
+ visionDistance = currentPose.getTranslation().getDistance(visionStartTranslation);
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Vision test drive distance", driveDistance);
+ SmartDashboard.putNumber("Vision test vision distnace", visionDistance);
+ SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance);
+ SmartDashboard.putNumber(
+ "Vision test % error", (visionDistance - driveDistance) / driveDistance * 100);
+ }
+
+ // If kPrintDelay seconds have passed, print the data
+ if (printTimer.advanceIfElapsed(PRINT_DELAY)) {
+ System.out.printf(
+ "\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
+ driveDistance,
+ visionDistance,
+ visionDistance - driveDistance,
+ (visionDistance - driveDistance) / driveDistance * 100);
+ }
+ } else {
+ endTimer.start();
+ }
+ }
+
+ /** Re-enables vision and stops the robot */
+ @Override
+ public void end(boolean interrupted) {
+ drive.setVisionEnabled(true);
+ drive.stop();
+ }
+
+ /**
+ * Returns if the command is finished
+ *
+ * @return If the end delay has elapsed
+ */
+ @Override
+ public boolean isFinished() {
+ return endTimer.hasElapsed(END_DELAY);
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+import org.wpilib.math.system.plant.DCMotor;
+import frc.robot.constants.swerve.DriveConstants;
+
+/** Container class for auto constants. */
+public class AutoConstants {
+
+ // Pathplanner output folder should be src/main/deploy/pathplanner
+ public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/";
+
+ public static final double MAX_AUTO_SPEED = 5.2; // m/s
+ public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2
+
+ public static RobotConfig CONFIG;
+ public static final PPHolonomicDriveController AUTO_CONTROLLER =
+ new PPHolonomicDriveController(
+ new PIDConstants(3.5, 0.0, 1.0), // Translation PID constants
+ new PIDConstants(4.0, 0.0, 1.0) // Rotation PID constants
+ );
+
+ static {
+ try {
+ CONFIG = RobotConfig.fromGUISettings();
+ } catch (Exception e) {
+ e.printStackTrace();
+ // Although these values are probably wrong and auto might not work correctly, at least it
+ // won't cause NullPointerExceptions
+ CONFIG =
+ new RobotConfig(
+ 50,
+ 0.5,
+ new ModuleConfig(
+ DriveConstants.WHEEL_RADIUS,
+ MAX_AUTO_SPEED,
+ DriveConstants.COSF,
+ DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO),
+ DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT,
+ 1),
+ DriveConstants.MODULE_LOCATIONS);
+ }
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import com.ctre.phoenix6.CANBus;
+
+import org.wpilib.framework.RobotBase;
+
+public class Constants {
+
+ // constants:
+
+ public static final double GRAVITY_ACCELERATION = 9.8;
+ public static final double ROBOT_VOLTAGE = 12.0;
+ public static final double LOOP_TIME = 0.02;
+
+ // CAN bus names
+ public static final CANBus CANIVORE_CAN = new CANBus("CANivore");
+ public static final CANBus CANIVORE_SUB = new CANBus("CANivoreSub");
+ public static final CANBus RIO_CAN = new CANBus("rio");
+
+ // Logging
+ public static final boolean USE_TELEMETRY = true;
+
+ // this would disable all logger calls
+ public static final boolean DISABLE_LOGGING = true;
+ public static final boolean DISABLE_SMART_DASHBOARD = true; // wont disable auto picker
+
+ public static enum Mode {
+ /** Running on a real robot. */
+ REAL,
+
+ /** Running a physics simulator. */
+ SIM,
+
+ /** Replaying from a log file. */
+ REPLAY
+ }
+
+ // Kraken Speed
+ public static double MAX_RPM = 5800.0; // Rotations per minute
+
+ /*
+ * Talon Stator / Supply Limits explanation
+ * Supply current is current that's being drawn at the input bus voltage. Stator
+ * current is current that's being drawn by the motor.
+ * Supply limiting (supported by Talon FX and SRX) is useful for preventing
+ * breakers from tripping in the PDP.
+ * Stator limiting (supported by Talon FX) is useful for limiting
+ * acceleration/heat.
+ */
+
+ // These are the default values
+
+ // Stator
+ public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp)
+ public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp)
+ public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s)
+
+ // Supply
+ public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_SUPPLY_CURRENT_LIMIT =
+ 40; // Limit(amp), current to hold after trigger hit
+ public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD =
+ 55; // (amp), amps to activate trigger
+ public static final double TALONFX_SUPPLY_TRIGGER_DURATION =
+ 3; // (s), how long after trigger before reducing
+
+ // OIConstants:
+
+ public static final int DRIVER_JOY = 0;
+ public static final int OPERATOR_JOY = 1;
+ public static final int TEST_JOY = 2;
+ public static final int MANUAL_JOY = 3;
+ public static final double DEFAULT_DEADBAND = 0.00005;
+
+ public static final double TRANSLATIONAL_DEADBAND = 0.01;
+
+ public static final double ROTATION_DEADBAND = 0.01;
+
+ public static final double HEADING_DEADBAND = 0.05;
+ public static final double HEADING_SLEWRATE = 10;
+
+ // Modes
+ public static final Mode SIM_MODE = Mode.SIM;
+ public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
+
+ // Enables 3D logs of mechanisms
+ public static final boolean LOG_MECHANISMS = true;
+
+ // Network setting for vision
+ public static final String VISION_CAMERA_HOST = "10.9.72.12";
+}
--- /dev/null
+package frc.robot.constants;
+
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFields;
+
+public class FieldConstants {
+
+ /** Apriltag layout for 2026 REBUILT */
+ public static final AprilTagFieldLayout field =
+ AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+ /** Width of the field [meters] */
+ public static final double FIELD_LENGTH = field.getFieldLength();
+
+ /** Height of the field [meters] */
+ public static final double FIELD_WIDTH = field.getFieldWidth();
+}
--- /dev/null
+package frc.robot.constants;
+
+/** constants for gyro bias estimation and correction via vision. */
+public class GyroBiasConstants {
+ /** minimum samples before applying correction */
+ public static final int MIN_SAMPLES = 10;
+
+ /** maximum angle difference to accept in radians */
+ public static final double MAX_ANGLE_DIFF_RAD = Math.toRadians(45);
+
+ /** minimum correction to apply in radians */
+ public static final double MIN_CORRECTION_RAD = Math.toRadians(0.1);
+
+ /** fraction of the correction to apply (0.0 to 1.0) */
+ public static final double CORRECTION_FRACTION = 0.2;
+
+ /** maximum correction per cycle in radians */
+ public static final double MAX_CORRECTION_PER_CYCLE_RAD = Math.toRadians(5);
+
+ /** alpha for exponential moving average 0.0 to 1.0, higher is more responsive */
+ public static final double EMA_ALPHA = 0.3;
+
+ /** min total weight required for weighted average */
+ public static final double MIN_TOTAL_WEIGHT = 3.0;
+}
--- /dev/null
+package frc.robot.constants;
+
+public class IdConstants {
+ // Drivetrain
+ public static final int DRIVE_FRONT_LEFT_ID = 1;
+ public static final int STEER_FRONT_LEFT_ID = 2;
+ public static final int ENCODER_FRONT_LEFT_ID = 3;
+ public static final int DRIVE_FRONT_RIGHT_ID = 10;
+ public static final int STEER_FRONT_RIGHT_ID = 11;
+ public static final int ENCODER_FRONT_RIGHT_ID = 12;
+ public static final int DRIVE_BACK_LEFT_ID = 7;
+ public static final int STEER_BACK_LEFT_ID = 8;
+ public static final int ENCODER_BACK_LEFT_ID = 9;
+ public static final int DRIVE_BACK_RIGHT_ID = 4;
+ public static final int STEER_BACK_RIGHT_ID = 5;
+ public static final int ENCODER_BACK_RIGHT_ID = 6;
+ public static final int PIGEON = 13;
+
+ // LEDs
+ public static final int CANDLE_ID = 1;
+
+ // Turret
+ public static final int TURRET_MOTOR_ID = 5;
+ public static final int TURRET_ENCODER_LEFT_ID = 6;
+ public static final int TURRET_ENCODER_RIGHT_ID = 7;
+
+ // Shooter
+ public static final int SHOOTER_LEFT_ID = 9;
+ public static final int SHOOTER_RIGHT_ID = 10;
+
+ // Hood
+ public static final int HOOD_ID = 11;
+
+ // Spindexer
+ public static final int SPINDEXER_ONE_ID = 4;
+ public static final int SPINDEXER_TWO_ID = 8;
+
+ // Intake
+ public static final int RIGHT_MOTOR_ID = 1;
+ public static final int LEFT_MOTOR_ID = 2;
+ public static final int ROLLER_MOTOR_ID = 3;
+}
--- /dev/null
+package frc.robot.constants;
+
+/** Container class for test constants. */
+public class TestConstants {
+ public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6;
+}
--- /dev/null
+package frc.robot.constants;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.util.Pair;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.math.util.Units;
+
+/** Container class for vision constants. */
+public class VisionConstants {
+ /** If April tag vision is enabled on the robot */
+ public static final boolean ENABLED = true;
+
+ /** If object detection should be enabled */
+ public static final boolean OBJECT_DETECTION_ENABLED = false;
+
+ /** If odometry should be updated using vision during auto */
+ public static final boolean ENABLED_AUTO = true;
+
+ /**
+ * If odometry should be updated using vision while running the GoToPose, GoToPosePID, and
+ * DriveToPose commands in teleop
+ */
+ public static final boolean ENABLED_GO_TO_POSE = true;
+
+ /** If vision should be simulated */
+ public static final boolean ENABLED_SIM = false;
+
+ /** If vision should only return values if it can see 2 good targets */
+ public static final boolean ONLY_USE_2_TAGS = false;
+
+ /** PoseStrategy to use in pose estimation */
+ public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+ /** Fallback PoseStrategy if MultiTag doesn't work */
+ public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY;
+
+ /** Any April tags we always want to ignore. To ignore a tag, put its id in this array. */
+ public static final int[] TAGS_TO_IGNORE = {};
+
+ /**
+ * If multiple cameras return different poses, they will be ignored if the difference between them
+ * is greater than this
+ */
+ public static final double MAX_POSE_DIFFERENCE = 0.2;
+
+ /** The maximum distance to the tag to use */
+ public static final double MAX_DISTANCE = 6;
+
+ /**
+ * If vision should use manual calculations (yawFunction-based vs referencePose-based). Changed to
+ * false to support gyro bias correction.
+ */
+ public static final boolean USE_MANUAL_CALCULATIONS = false;
+
+ // <ol start="0"> did not work
+ /**
+ * Which version of driver assist to use. This would be an enum, except there is no short and
+ * descriptive name for all of these.
+ *
+ * <p>The options are:
+ *
+ * <p>0: Disable driver assist
+ *
+ * <p>1: Completely remove the component of the driver's input that is not toward the object
+ *
+ * <p>2: Interpolate between the next achievable driver speed and a speed calculated using
+ * trapezoid profiles
+ *
+ * <p>3-5: Add a speed perpendicular to the driver input; there are 3 similar but different
+ * calculations for this
+ */
+ public static final int DRIVER_ASSIST_MODE = 5;
+
+ /**
+ * The number to multiply the distance to the April tag by.
+ *
+ * <p>Only affects manual calculations.
+ *
+ * <p>To find this, set it to 1 and measure the actual distance and the calculated distance.
+ *
+ * <p>This should not be needed, and it is only here because it improved the accuracy of vision in
+ * the 2023 fall semester
+ */
+ public static final double DISTANCE_SCALE = 1;
+
+ /** The standard deviations to use for vision */
+ public static final Matrix<N3, N1> VISION_STD_DEVS =
+ VecBuilder.fill(
+ 0.3, // x in meters (default=0.9)
+ 0.3, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /** The standard deviations to use for vision when the wheels slip */
+ public static final Matrix<N3, N1> VISION_STD_DEVS_2 =
+ VecBuilder.fill(
+ 0.01, // x in meters (default=0.9)
+ 0.01, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /**
+ * The highest ambiguity to use. Ambiguities higher than this will be ignored.
+ *
+ * <p>Only affects calculations using PhotonVision, not manual calculations.
+ */
+ public static final double HIGHEST_AMBIGUITY = 0.05;
+
+ public static final int MAX_EMPTY_TICKS = 10;
+
+ /**
+ * The camera poses
+ *
+ * <p>Everything is in meters and radians
+ *
+ * <p>0 for all numbers is center of the robot, on the ground, looking straight toward the front
+ *
+ * <p>+ X: Front of Robot
+ *
+ * <p>+ Y: Left of Robot
+ *
+ * <p>+ Z: Top of Robot
+ *
+ * <p>+ Pitch: Down
+ *
+ * <p>+ Yaw: Counterclockwise
+ */
+ public static final ArrayList<Pair<String, Transform3d>> APRIL_TAG_CAMERAS =
+ new ArrayList<Pair<String, Transform3d>>(
+ List.of(
+ new Pair<String, Transform3d>(
+ "CameraFrontLeft",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraFrontRight",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(-11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackLeft",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(145.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackRight",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(-12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-145.0))))));
+
+ /** The transformations from the robot to object detection cameras */
+ public static final ArrayList<Transform3d> OBJECT_DETECTION_CAMERAS =
+ new ArrayList<>(
+ List.of(
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
+ new Rotation3d(0, Units.degreesToRadians(20), 0))));
+
+ // used to cleanly shutdown the OrangePi
+ public static final String[] ORANGEPI_HOSTNAMES = {"photonfront.local", "photonback.local"};
+ public static final String ORANGEPI_USERNAME = "pi";
+ public static final String ORANGEPI_PASSWORD = "raspberry";
+}
--- /dev/null
+package frc.robot.constants.swerve;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.util.Units;
+import frc.robot.RobotId;
+import frc.robot.constants.Constants;
+import frc.robot.util.SwerveStuff.ModuleLimits;
+import lib.COTSFalconSwerveConstants;
+
+/**
+ * Global constants are, by default, for the competition robot. Global constants get changed in the
+ * update method if the RobotId detected is not the competition robot.
+ */
+public class DriveConstants {
+ /**
+ * The robot's width with its bumpers on.
+ *
+ * <p>The frame width is 26.5 inches, and each bumper is 3.25 inches.
+ */
+ public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.83185; // 32.75 inches in meters
+
+ public static double ROBOT_MASS = Units.lbsToKilograms(111.6 + 13 + 13.4 + 5.0);
+
+ /** Radius of the drive wheels [meters]. */
+ public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);
+
+ public static double WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ /** Distance between the left and right wheels [meters]. */
+ // from center of wheels btw
+ public static double TRACK_WIDTH =
+ Units.inchesToMeters(20.75); // 22.75 swerve bot, 20.75 comp bot
+
+ // Mk4i gear ratios
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ // standard gear ratios
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ // changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0).
+ /** Drive gear ratio for an Mk4i with L2-Plus gearing */
+ public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ // all MK4i modules have the same steering gear ratio
+ public static double STEER_GEAR_RATIO = 150.0 / 7.0;
+
+ /**
+ * Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius
+ */
+ public static final double MAX_SPEED = 4.5;
+
+ // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the
+ // radians/s of the robot)
+ // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal =
+ // sqrt(2) * side_length.
+ public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2));
+
+ public static final double COSF = 1.5;
+
+ // The maximum acceleration of the robot, limited by friction
+ public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION;
+ // The maximum amount a drive motor can accelerate, independant of friction
+ // This does nothing if greater than LINEAR_ACCEL
+ public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL;
+ // The maximum angular acceleration of the robot
+ public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2);
+
+ /**
+ * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint.
+ *
+ * <p>If this is true, Drivetrain will use the actual current setpoint instead.
+ */
+ public static final boolean USE_ACTUAL_SPEED = false;
+
+ /**
+ * Disables the deadband and optimization for the modules. SwerveSetpointGenerator adds its own
+ * optimization and deadband, and the controllers also have a deadband. Setting this to true fixes
+ * bugs caused by using hte actual current state.
+ */
+ public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false;
+
+ public static final Rotation2d STARTING_HEADING = new Rotation2d();
+
+ public static final Translation2d[] MODULE_LOCATIONS = {
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2)
+ };
+
+ public static final SwerveDriveKinematics KINEMATICS =
+ new SwerveDriveKinematics(MODULE_LOCATIONS);
+
+ /**
+ * Default values for SwerveCompetition drivetrain Sets to correct value later if robotID is
+ * different
+ */
+ public static double STEER_OFFSET_FRONT_LEFT = 302.646;
+
+ public static double STEER_OFFSET_FRONT_RIGHT = 103.039 + 180;
+ public static double STEER_OFFSET_BACK_LEFT = 165.49 + 90;
+ public static double STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ // Heading PID.
+ public static final double HEADING_P = 5.5;
+ public static final double HEADING_D = 0;
+
+ public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5);
+
+ // Translational PID
+ // TODO: Tune this better (low priority since we aren't using it in 2025)
+ public static final double TRANSLATIONAL_P = 1;
+ public static final double TRANSLATIONAL_D = 0.001;
+
+ // The PIDs for PathPlanner Command
+ public static final double PATH_PLANNER_HEADING_P = 3.5 / 2;
+ public static final double PATH_PLANNER_HEADING_D = 0;
+
+ public static final double PATH_PLANNER_TRANSLATIONAL_P = 6 / 2;
+ public static final double PATH_PLANNER_TRANSLATIONAL_D = 0;
+
+ // CAN
+ public static CANBus DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_ENCODER_CAN = Constants.CANIVORE_CAN;
+ public static CANBus PIGEON_CAN = Constants.CANIVORE_CAN;
+
+ public static COTSFalconSwerveConstants MODULE_CONSTANTS =
+ COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ /* Swerve Current Limiting */
+ public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15;
+ public static final int STEER_PEAK_CURRENT_LIMIT = 15;
+ public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
+
+ public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40;
+ public static final int DRIVE_PEAK_CURRENT_LIMIT = 40;
+ public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
+
+ /* Motor inversions */
+ public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive;
+ public static InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive;
+
+ /* Neutral Modes */
+ public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
+ public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
+
+ /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
+ public static double GYRO_MOUNT_POSE_ROLL = 0.0;
+
+ /* Drive Motor PID Values */
+ public static final double[] P_VALUES = {0.3, 0.3, 0.3, 0.3};
+ public static final double[] I_VALUES = {0, 0, 0, 0};
+ public static final double[] D_VALUES = {0, 0, 0, 0};
+ /* Drive Motor Characterization Values
+ * Divide SYSID values by 12 to convert from volts to percent output for CTRE */
+ public static final double[] S_VALUES = {0.11, 0.11, 0.11, 0.11};
+ public static final double[] V_VALUES = {0.11079, 0.10718, 0.11009, 0.1164};
+ public static final double[] A_VALUES = {0.005482, 0.0049593, 0.010156, 0.0065708};
+ /* Ramp values for drive motors in open loop driving. */
+ // Open loop prevents throttle from changing too quickly.
+ // It will limit it to time given (in seconds) to go from zero to full throttle.
+ // A small open loop ramp (0.25) helps with tread wear, tipping, etc
+ public static final double OPEN_LOOP_RAMP = 0.1;
+
+ // limits maximum rate of change for motor
+ public static final double CLOSE_LOOP_RAMP = 0.0;
+
+ public static final double WHEEL_CIRCUMFERENCE = 2 * Math.PI * WHEEL_RADIUS;
+
+ public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW-
+
+ public static final double SLOW_DRIVE_FACTOR = 0.2;
+ public static final double SLOW_ROT_FACTOR = 0.1;
+
+ public static final ModuleLimits MODULE_LIMITS =
+ new ModuleLimits(
+ MAX_SPEED,
+ MAX_DRIVE_ACCEL,
+ COSF,
+ Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO));
+
+ /** Updates the constants if the RobotId is not the default SwerveCompetition robot. */
+ public static void update(RobotId robotId) {
+ if (robotId == RobotId.PrimeJr) {
+ STEER_OFFSET_FRONT_LEFT = 187.64 + 180; // module zero
+ STEER_OFFSET_FRONT_RIGHT = 162 + 180 + 180; // module one
+ STEER_OFFSET_BACK_LEFT = 196.3 + 180; // module two
+ STEER_OFFSET_BACK_RIGHT = 357 + 180 + 180; // module three
+
+ // MK5n
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ // Gear ratios
+ // DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio
+ DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); // R1 Ratio
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ // Gyro is mounted under the robot
+ GYRO_MOUNT_POSE_ROLL = 180.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.TwinBot) {
+ STEER_OFFSET_FRONT_LEFT = 131.201172;
+ STEER_OFFSET_FRONT_RIGHT = 247.324219;
+ STEER_OFFSET_BACK_LEFT = 39.814463;
+ STEER_OFFSET_BACK_RIGHT = 294.873047;
+
+ // MK5n gear ratio
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.SwerveCompetition) {
+ STEER_OFFSET_FRONT_LEFT = 302.646;
+ STEER_OFFSET_FRONT_RIGHT = 103.039 + 180;
+ STEER_OFFSET_BACK_LEFT = 165.49 + 90;
+ STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.BetaBot) {
+ STEER_OFFSET_FRONT_LEFT = 193.884 - 180;
+ STEER_OFFSET_FRONT_RIGHT = 110.914;
+ STEER_OFFSET_BACK_LEFT = 128.054 + 180;
+ STEER_OFFSET_BACK_RIGHT = 107.43;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vivace) {
+ STEER_OFFSET_FRONT_LEFT = 100.184 + 180;
+ STEER_OFFSET_FRONT_RIGHT = 224.293;
+ STEER_OFFSET_BACK_LEFT = 304.795 - 180;
+ STEER_OFFSET_BACK_RIGHT = 201.177 - 180;
+
+ ROBOT_MASS = 50;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vertigo) {
+ STEER_OFFSET_FRONT_LEFT = Units.radiansToDegrees(3.43);
+ STEER_OFFSET_FRONT_RIGHT = Units.radiansToDegrees(1.91) + 180;
+ STEER_OFFSET_BACK_LEFT = Units.radiansToDegrees(2.28);
+ STEER_OFFSET_BACK_RIGHT = Units.radiansToDegrees(5.03);
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ ROBOT_MASS = 20;
+
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ // Falcon Speed
+ Constants.MAX_RPM = 6080.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Phil) {
+ ROBOT_MASS = 30;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ STEER_OFFSET_FRONT_LEFT = 121.463 + 180;
+ STEER_OFFSET_FRONT_RIGHT = 284.242;
+ STEER_OFFSET_BACK_LEFT = 157.676;
+ STEER_OFFSET_BACK_RIGHT = 77.199;
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else {
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+ }
+ }
+}
--- /dev/null
+package frc.robot.constants.swerve;
+
+import frc.robot.constants.IdConstants;
+
+/**
+ * Container class for module constants, defined using constants from {@link DriveConstants} .
+ *
+ * @see DriveConstants
+ */
+public enum ModuleConstants {
+ FRONT_LEFT(
+ IdConstants.DRIVE_FRONT_LEFT_ID,
+ IdConstants.STEER_FRONT_LEFT_ID,
+ IdConstants.ENCODER_FRONT_LEFT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_LEFT,
+ ModuleType.FRONT_LEFT,
+ DriveConstants.S_VALUES[0],
+ DriveConstants.V_VALUES[0],
+ DriveConstants.A_VALUES[0],
+ DriveConstants.P_VALUES[0],
+ DriveConstants.I_VALUES[0],
+ DriveConstants.D_VALUES[0]),
+ FRONT_RIGHT(
+ IdConstants.DRIVE_FRONT_RIGHT_ID,
+ IdConstants.STEER_FRONT_RIGHT_ID,
+ IdConstants.ENCODER_FRONT_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_RIGHT,
+ ModuleType.FRONT_RIGHT,
+ DriveConstants.S_VALUES[1],
+ DriveConstants.V_VALUES[1],
+ DriveConstants.A_VALUES[1],
+ DriveConstants.P_VALUES[1],
+ DriveConstants.I_VALUES[1],
+ DriveConstants.D_VALUES[1]),
+ BACK_LEFT(
+ IdConstants.DRIVE_BACK_LEFT_ID,
+ IdConstants.STEER_BACK_LEFT_ID,
+ IdConstants.ENCODER_BACK_LEFT_ID,
+ DriveConstants.STEER_OFFSET_BACK_LEFT,
+ ModuleType.BACK_LEFT,
+ DriveConstants.S_VALUES[2],
+ DriveConstants.V_VALUES[2],
+ DriveConstants.A_VALUES[2],
+ DriveConstants.P_VALUES[2],
+ DriveConstants.I_VALUES[2],
+ DriveConstants.D_VALUES[2]),
+ BACK_RIGHT(
+ IdConstants.DRIVE_BACK_RIGHT_ID,
+ IdConstants.STEER_BACK_RIGHT_ID,
+ IdConstants.ENCODER_BACK_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_BACK_RIGHT,
+ ModuleType.BACK_RIGHT,
+ DriveConstants.S_VALUES[3],
+ DriveConstants.V_VALUES[3],
+ DriveConstants.A_VALUES[3],
+ DriveConstants.P_VALUES[3],
+ DriveConstants.I_VALUES[3],
+ DriveConstants.D_VALUES[3]),
+
+ NONE(0, 0, 0, 0.0, ModuleType.NONE, 0, 0, 0, 0, 0, 0);
+
+ private final int drivePort;
+ private final int steerPort;
+ private final int encoderPort;
+ private final double steerOffset;
+ private final double ks;
+ private final double kv;
+ private final double ka;
+ private final double driveP;
+ private final double driveI;
+ private final double driveD;
+ private final ModuleType type;
+
+ ModuleConstants(
+ int drivePort,
+ int steerPort,
+ int encoderPort,
+ double steerOffset,
+ ModuleType type,
+ double ks,
+ double kv,
+ double ka,
+ double driveP,
+ double driveI,
+ double driveD) {
+
+ this.drivePort = drivePort;
+ this.steerPort = steerPort;
+ this.encoderPort = encoderPort;
+ this.steerOffset = steerOffset;
+ this.type = type;
+ this.ks = ks;
+ this.kv = kv;
+ this.ka = ka;
+ this.driveP = driveP;
+ this.driveI = driveI;
+ this.driveD = driveD;
+ }
+
+ public int getDrivePort() {
+ return drivePort;
+ }
+
+ public int getSteerPort() {
+ return steerPort;
+ }
+
+ public int getEncoderPort() {
+ return encoderPort;
+ }
+
+ public double getSteerOffset() {
+ return steerOffset;
+ }
+
+ public ModuleType getType() {
+ return type;
+ }
+
+ public double getDriveS() {
+ return ks;
+ }
+
+ public double getDriveV() {
+ return kv;
+ }
+
+ public double getDriveA() {
+ return ka;
+ }
+
+ public double getDriveP() {
+ return driveP;
+ }
+
+ public double getDriveI() {
+ return driveI;
+ }
+
+ public double getDriveD() {
+ return driveD;
+ }
+}
--- /dev/null
+package frc.robot.constants.swerve;
+
+/**
+ * Represents the type for a module on the robot.
+ *
+ * <p>IDs: 0 - FRONT_LEFT 1 - FRONT_RIGHT 2 - BACK_LEFT 3 - BACK_RIGHT
+ */
+public enum ModuleType {
+ FRONT_LEFT,
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT,
+ NONE;
+
+ public final byte id;
+
+ ModuleType() {
+ this.id = id();
+ }
+
+ private byte id() {
+ if (this == NONE) return -1;
+ // This is a trick that relies on the order the enums are defined.
+ return (byte) this.ordinal();
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.system.RobotController;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.DynamicSlewRateLimiter;
+import frc.robot.util.MathUtils;
+
+/** Abstract class for different controller types. */
+public abstract class BaseDriverConfig {
+
+ protected final Drivetrain drive;
+
+ private double previousHeading = 0;
+
+ private final DynamicSlewRateLimiter headingLimiter =
+ new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE);
+
+ /**
+ * @param drive the drivetrain instance
+ * @param controllerTab the shuffleboard controller tab
+ * @param shuffleboardUpdates whether to update the shuffleboard
+ */
+ public BaseDriverConfig(Drivetrain drive) {
+ headingLimiter.setContinuousLimits(-Math.PI, Math.PI);
+ headingLimiter.enableContinuous(true);
+ this.drive = drive;
+ }
+
+ public double getForwardTranslation() {
+ double forward = getRawForwardTranslation();
+ return forward
+ * DriveConstants.MAX_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12)
+ * MathUtil.applyDeadband(
+ Math.sqrt(forward * forward + Math.pow(getRawSideTranslation(), 2)),
+ Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getSideTranslation() {
+ double side = getRawSideTranslation();
+ return side
+ * DriveConstants.MAX_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12)
+ * MathUtil.applyDeadband(
+ Math.sqrt(side * side + Math.pow(getRawForwardTranslation(), 2)),
+ Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getRotation() {
+ return MathUtils.expoMS(
+ MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2)
+ * DriveConstants.MAX_ANGULAR_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12);
+ }
+
+ public double getHeading() {
+ if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND)
+ return headingLimiter.calculate(previousHeading, 1e-6);
+ previousHeading =
+ headingLimiter.calculate(
+ getRawHeadingAngle(), MathUtils.expoMS(getRawHeadingMagnitude(), 2));
+ return previousHeading;
+ }
+
+ protected Drivetrain getDrivetrain() {
+ return drive;
+ }
+
+ /** Configures the controls for the controller. */
+ public abstract void configureControls();
+
+ public abstract double getRawSideTranslation();
+
+ public abstract double getRawForwardTranslation();
+
+ public abstract double getRawRotation();
+
+ public abstract double getRawHeadingAngle();
+
+ public abstract double getRawHeadingMagnitude();
+
+ public abstract boolean getIsSlowMode();
+
+ public abstract boolean getIsAlign();
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import org.wpilib.command2.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.Ex3DProController;
+import lib.controllers.Ex3DProController.Ex3DProAxis;
+import lib.controllers.Ex3DProController.Ex3DProButton;
+
+/** Driver controls for the Ex3D Pro controller. */
+public class Ex3DProDriverConfig extends BaseDriverConfig {
+
+ private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11);
+
+ public Ex3DProDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver
+ .get(Ex3DProButton.B2)
+ .onTrue(
+ new InstantCommand(
+ () -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return -kDriver.get(Ex3DProAxis.X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(Ex3DProAxis.Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(Ex3DProAxis.Z);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(Ex3DProAxis.Z) * Math.PI;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(Ex3DProAxis.SLIDER);
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.command2.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.GameController;
+import lib.controllers.GameController.Axis;
+import lib.controllers.GameController.Button;
+
+/** Driver controls for the generic game controller. */
+public class GameControllerDriverConfig extends BaseDriverConfig {
+ private final GameController driver = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY);
+
+ public GameControllerDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ // Reset yaw to be away from driver
+ driver
+ .get(Button.START)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ super.getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ driver
+ .get(driver.RIGHT_TRIGGER_BUTTON)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return driver.get(Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return driver.get(Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return driver.get(Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(driver.get(Axis.RIGHT_X), -driver.get(Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(driver.get(Axis.RIGHT_X), driver.get(Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ // return kDriver.LEFT_TRIGGER_BUTTON.getAsBoolean();
+ }
+
+ public GameController getGameController() {
+ return driver;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import org.wpilib.command2.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.MadCatzController;
+import lib.controllers.MadCatzController.MadCatzAxis;
+import lib.controllers.MadCatzController.MadCatzButton;
+
+/** Driver controls for the MadCatz controller. */
+public class MadCatzDriverConfig extends BaseDriverConfig {
+
+ private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6);
+
+ public MadCatzDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver
+ .get(MadCatzButton.B2)
+ .onTrue(
+ new InstantCommand(
+ () -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return kDriver.get(MadCatzAxis.X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(MadCatzAxis.Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(MadCatzAxis.ZROTATE);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(MadCatzAxis.SLIDER);
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
--- /dev/null
+// 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.controls;
+
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.command2.InstantCommand;
+import org.wpilib.command2.button.Trigger;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.GameController;
+
+/** Controls for the operator, which are almost a duplicate of most of the driver's controls */
+public class Operator {
+
+ private final GameController driver = new GameController(Constants.OPERATOR_JOY);
+
+ private final Drivetrain drive;
+
+ public Operator(Drivetrain drive) {
+ this.drive = drive;
+ }
+
+ public void configureControls() {
+ // Cancel commands, could be removed if the operator doesn't need this button
+ driver
+ .get(driver.RIGHT_TRIGGER_BUTTON)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ drive.setIsAlign(false);
+ drive.setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ public Trigger getRightTrigger() {
+ return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
+ }
+
+ public Trigger getLeftTrigger() {
+ return new Trigger(driver.LEFT_TRIGGER_BUTTON);
+ }
+
+ public GameController getGameController() {
+ return driver;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.command2.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.PS5Axis;
+import lib.controllers.PS5Controller.PS5Button;
+
+/** Driver controls for the PS5 controller */
+public class PS5ControllerDriverConfig extends BaseDriverConfig {
+ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+
+ public PS5ControllerDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller
+ .get(PS5Button.CREATE)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller
+ .get(PS5Button.RB)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y))
+ - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.command2.FunctionalCommand;
+import org.wpilib.command2.InstantCommand;
+import org.wpilib.command2.SequentialCommandGroup;
+import org.wpilib.command2.WaitCommand;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.GameController;
+import lib.controllers.GameController.Axis;
+import lib.controllers.GameController.Button;
+import lib.controllers.GameController.DPad;
+
+/**
+ * Driver config for PS5 controllers using Xbox 360 emulation mode. This lets SCUF and other PS5
+ * controllers work with WPILib rumble.
+ *
+ * <p>Setup: - download DSX (https://dualsensex.com/download/) - install ViGEmBus driver (if app
+ * doesn't auto prompt) - in dsx, set "controller emulation" to Xbox 360 - ensure rumble is enabled
+ * in dsx settings - once code is depoloyed, change controller to "Xbox 360" in driverstation
+ */
+public class PS5XboxModeDriverConfig extends BaseDriverConfig {
+ private final GameController controller = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Command reverseMotors = null;
+
+ // PS5 button aliases
+ // private final Button CROSS = Button.A;
+ private final Button CIRCLE = Button.B;
+ private final Button SQUARE = Button.X;
+ // private final Button TRIANGLE = Button.Y;
+ // private final Button LB = Button.LB;
+ private final Button RB = Button.RB;
+ private final Button CREATE = Button.BACK;
+ // private final Button OPTIONS = Button.START;
+ private final Button LEFT_JOY = Button.LEFT_JOY;
+ private final Button RIGHT_JOY = Button.RIGHT_JOY;
+
+ // PS5 trigger buttons
+ private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
+ private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
+
+ // PS5 axis aliases
+ private final Axis LEFT_X = Axis.LEFT_X;
+ private final Axis LEFT_Y = Axis.LEFT_Y;
+ private final Axis RIGHT_X = Axis.RIGHT_X;
+ private final Axis RIGHT_Y = Axis.RIGHT_Y;
+
+ // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+
+ public PS5XboxModeDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller
+ .get(CREATE)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller
+ .get(RB)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Align wheels
+ controller
+ .get(DPad.RIGHT)
+ .onTrue(
+ new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false,
+ getDrivetrain())
+ .withTimeout(2));
+
+ // Rumble test
+ controller
+ .get(RIGHT_JOY)
+ .onTrue(
+ new SequentialCommandGroup(
+ new InstantCommand(
+ () -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(
+ () -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+ public void startRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ }
+
+ public void endRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
+ }
+}
--- /dev/null
+package frc.robot.subsystems.LED;
+
+import com.ctre.phoenix6.configs.CANdleConfigurator;
+import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
+import com.ctre.phoenix6.configs.LEDConfigs;
+import com.ctre.phoenix6.controls.ColorFlowAnimation;
+import com.ctre.phoenix6.controls.FireAnimation;
+import com.ctre.phoenix6.controls.RainbowAnimation;
+import com.ctre.phoenix6.controls.RgbFadeAnimation;
+import com.ctre.phoenix6.controls.SolidColor;
+import com.ctre.phoenix6.controls.StrobeAnimation;
+import com.ctre.phoenix6.controls.TwinkleAnimation;
+import com.ctre.phoenix6.hardware.CANdle;
+import com.ctre.phoenix6.signals.Enable5VRailValue;
+import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
+import com.ctre.phoenix6.signals.RGBWColor;
+import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
+import com.ctre.phoenix6.signals.StripTypeValue;
+import com.ctre.phoenix6.signals.VBatOutputModeValue;
+
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import org.wpilib.util.Color;
+import org.wpilib.command2.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class LED extends SubsystemBase {
+
+ private CANdle candle;
+ public static final int stripLength = 67;
+
+ /// Hz
+ public static final int FLASH_RATE = 4;
+
+ private Color color;
+
+ public LED() {
+ candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
+ CANdleConfigurator configurator = candle.getConfigurator();
+
+ LEDConfigs ledConf =
+ new LEDConfigs()
+ .withStripType(StripTypeValue.GRB)
+ .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
+ .withBrightnessScalar(1);
+
+ CANdleFeaturesConfigs featureConf =
+ new CANdleFeaturesConfigs()
+ .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
+ .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
+ .withVBatOutputMode(VBatOutputModeValue.On);
+
+ configurator.apply(featureConf);
+ configurator.apply(ledConf);
+
+ setColor();
+
+ candle.clearAllAnimations();
+ lightsOff();
+
+ // System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+ }
+
+ public void setColor() {
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isEmpty()) {
+ color = Color.kOrangeRed;
+ } else if (alliance.get() == Alliance.Red) {
+ color = Color.kRed;
+ } else if (alliance.get() == Alliance.Blue) {
+ color = Color.kBlue;
+ } else {
+ color = Color.kOrangeRed;
+ }
+ }
+
+ private enum State {
+ OFF,
+ ON,
+ AUTO,
+ SLOW,
+ FAST,
+ ENDGAME
+ };
+
+ private State lastState = State.OFF;
+ private boolean forceOff = false;
+
+ @Override
+ public void periodic() {
+ State targetState = State.ON;
+ // if (underSecsToFlip(5)) targetState = State.SLOW;
+ // if (underSecsToFlip(1)) targetState = State.FAST;
+ if (DriverStation.isAutonomous()) targetState = State.AUTO;
+ if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
+ if (forceOff) targetState = State.OFF;
+
+ if (targetState != lastState) {
+ switch (targetState) {
+ case OFF:
+ lightsOff();
+ break;
+ case ON:
+ setStatic();
+ break;
+ case AUTO:
+ setTwinkle();
+ break;
+ case SLOW:
+ setStrobe();
+ break;
+ case FAST:
+ setFastStrobe();
+ break;
+ case ENDGAME:
+ setRainbow();
+ break;
+ }
+ lastState = targetState;
+ }
+ }
+
+ public void setFire() {
+ candle.clearAllAnimations();
+ candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
+ }
+
+ public void setRainbow() {
+ candle.clearAllAnimations();
+ candle.setControl(new RainbowAnimation(8, 8 + stripLength));
+ }
+
+ public void setRgbFadeAnimation() {
+ candle.clearAllAnimations();
+ candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
+ }
+
+ public void setTwinkle() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
+ }
+
+ public void setColorFlow() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
+ }
+
+ public void setStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new StrobeAnimation(8, 8 + stripLength)
+ .withFrameRate(FLASH_RATE)
+ .withColor(new RGBWColor(color)));
+ }
+
+ public void setFastStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new StrobeAnimation(8, 8 + stripLength)
+ .withFrameRate(FLASH_RATE * 4)
+ .withColor(new RGBWColor(color)));
+ }
+
+ public void setStatic() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
+ }
+
+ public void lightsOff() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
+ }
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import org.wpilib.system.RobotController;
+import org.wpilib.command2.SubsystemBase;
+
+public class Battery extends SubsystemBase {
+ private double voltage;
+
+ public Battery() {
+ updateVoltageFromBattery();
+ }
+
+ private void updateVoltageFromBattery() {
+ voltage = RobotController.getBatteryVoltage();
+ }
+
+ public double getVoltage() {
+ return voltage;
+ }
+
+ public double voltsTillBrownOut() {
+ return voltage - RobotController.getBrownoutVoltage();
+ }
+
+ public double toBrownOut() {
+ // percent of volts we've got left over what we had to start with
+ return voltsTillBrownOut()
+ / (BatteryConstants.MAX_STARTING_VOLTS - RobotController.getBrownoutVoltage());
+ }
+
+ @Override
+ public void periodic() {
+ updateVoltageFromBattery();
+ }
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+public class BatteryConstants {
+ public static final double MAX_STARTING_VOLTS = 12.5; // V
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import java.util.LinkedHashMap;
+import java.util.Map;
+
+public class BreakerConstants {
+ public static final Map<Double, Double> THRESHOLDS = new LinkedHashMap<>();
+
+ static {
+ THRESHOLDS.put(1.0, 6.0 * 120); // breaker default at 120
+ THRESHOLDS.put(4.0, 3.4 * 120);
+ THRESHOLDS.put(10.0, 2.0 * 120);
+ THRESHOLDS.put(20.0, 1.6 * 120);
+ THRESHOLDS.put(30.0, 1.5 * 120);
+ }
+
+ public static final double WARNING_PERCENTAGE =
+ 0.6; // percent that the system reacts to approaching thresholds
+
+ // ports
+ public static int[] DRIVETRAIN_PORTS = {
+ 8, 9, 10, 11, 18, 19, 0, 1
+ }; // bls, bld, fld, fls, frs, frd, brd, brs
+ public static int[] TURRET_PORTS = {2};
+ public static int[] INTAKE_PORTS = {15, 14, 13}; // right, left, roller
+ public static int[] SHOOTER_PORTS = {3, 4}; // left, right
+ public static int[] HOOD_PORTS = {5}; // shooter
+ public static int[] SPINDEXER_PORTS = {12}; // spindexer (unupdated on sheets)
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+
+import org.littletonrobotics.junction.Logger;
+
+import org.wpilib.hardware.power.PowerDistribution;
+import org.wpilib.command2.SubsystemBase;
+import frc.robot.constants.Constants;
+
+public class EMABreaker extends SubsystemBase {
+
+ private static class Current {
+ double tau;
+ double alpha; // how much of the error we correct per loop
+ double average = 0;
+ double threshold;
+ }
+
+ PowerDistribution pDis = new PowerDistribution();
+
+ double[] subsystemCurrents;
+
+ private List<Current> filters =
+ new ArrayList<>(); // contains currents with their alphas and thresholds
+ private List<Current> subsystems = new ArrayList<>();
+
+ public EMABreaker() {
+ for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
+ double tau = entry.getKey(); // sec
+ double threshold = entry.getValue(); // A
+
+ Current w = new Current(); // create a filter for the threshold
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha =
+ 1
+ - Math.exp(
+ -Constants.LOOP_TIME
+ / tau); // 1 - e^(-0.02/1) = 0.0198, 1 - e^(-0.02/2) = 0.00995
+
+ filters.add(w);
+ }
+
+ // subsystems
+ for (int i = 0; i < pDis.getNumChannels(); i++) {
+ double tau = 1.0;
+ double threshold = i;
+ Current w = new Current();
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.
+
+ subsystems.add(w);
+ }
+ }
+
+ @Override
+ public void periodic() {
+ double current = getCurrentFromPowerDistribution();
+ // this is total current averages
+ for (Current f : filters) {
+ // new avg = old avg + fractionAlpha * difference
+ f.average += f.alpha * (current - f.average);
+ Logger.recordOutput("Breaker/IntervalAverage/" + f.tau, f.average);
+ }
+
+ // this is getting currents coming out of all the ports from PDH (big thing
+ // under robot all the wires come out of)
+ subsystemCurrents = getAllCurrentFromPowerDistribution();
+
+ // this should average out all ports
+ for (Current s : subsystems) {
+ s.average += s.alpha * (subsystemCurrents[(int) s.threshold] - s.average);
+ }
+
+ // this should use updated port averages and sum them to get drivetrain average
+ // draw for 1 tau (can add more later)
+ Logger.recordOutput(
+ "Breaker/DrivetrainAverageDraw", getAverageCurrentDraw(BreakerConstants.DRIVETRAIN_PORTS));
+ Logger.recordOutput(
+ "Breaker/SpindexerDraw", getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS));
+ Logger.recordOutput(
+ "Breaker/ShooterDraw", getAverageCurrentDraw(BreakerConstants.SHOOTER_PORTS));
+ Logger.recordOutput("Breaker/IntakeDraw", getAverageCurrentDraw(BreakerConstants.INTAKE_PORTS));
+ Logger.recordOutput("Breaker/TurretDraw", getAverageCurrentDraw(BreakerConstants.TURRET_PORTS));
+ Logger.recordOutput("Breaker/HoodDraw", getAverageCurrentDraw(BreakerConstants.HOOD_PORTS));
+
+ // total stuff
+ Logger.recordOutput("Breaker/TotalCurrent", current);
+ Logger.recordOutput("Breaker/CurrentWarning", isInWarning());
+ }
+
+ public double getAverageCurrentDraw(int[] ports) {
+ double sum = 0;
+ for (int number : ports) {
+ sum += subsystems.get(number).average;
+ }
+ return sum;
+ }
+
+ public double getCurrentFromPowerDistribution() {
+ return pDis
+ .getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can
+ // get just
+ // one port
+ }
+
+ public double[] getAllCurrentFromPowerDistribution() {
+ return pDis.getAllCurrents();
+ }
+
+ public boolean isInWarning() {
+ for (Current f : filters) {
+ if (f.average > f.threshold * BreakerConstants.WARNING_PERCENTAGE) {
+ return true; // uh oh
+ }
+ }
+ return false;
+ }
+
+ // returns an average of the filters
+ public double percentageAverageUsage() {
+ double sumAvg = 0;
+ for (Current f : filters) {
+ sumAvg += f.average / f.threshold; // gets percentage of us
+ }
+ return sumAvg / filters.size(); // average across filters
+ }
+
+ // gives the worst case filter
+ public double[] percentageMaxUsage() {
+ Current worst = filters.get(0); // returns worst (default to tau filter)
+ for (Current f : filters) {
+ if (f.average / f.threshold > worst.average / worst.threshold) {
+ worst = f;
+ }
+ }
+ double[] returnValue = {worst.average / worst.threshold, worst.tau};
+ return returnValue;
+ }
+}
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Arrays;
+import java.util.ArrayList;
+import java.util.Optional;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.pathplanner.lib.util.PathPlannerLogging;
+
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Voltage;
+import org.wpilib.framework.RobotBase;
+import org.wpilib.system.Timer;
+import org.wpilib.smartdashboard.Field2d;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.command2.SubsystemBase;
+import frc.robot.commands.Music;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.GyroBiasConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.PhoenixOdometryThread;
+import frc.robot.util.SwerveModulePose;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+import frc.robot.util.Vision.Vision;
+import frc.robot.util.Vision.GyroBiasEstimator;
+import org.photonvision.EstimatedRobotPose;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ *
+ * <p>Module IDs are: 1: Front left 2: Front right 3: Back left 4: Back right
+ */
+public class Drivetrain extends SubsystemBase {
+
+ protected final Module[] modules;
+
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+
+ public static Lock odometryLock = new ReentrantLock();
+
+ private SwerveSetpoint currentSetpoint =
+ new SwerveSetpoint(
+ new ChassisSpeeds(),
+ new SwerveModuleState[] {
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState()
+ });
+ // Odometry
+ private final SwerveDrivePoseEstimator poseEstimator;
+
+ // Vision
+ private final Vision vision;
+
+ // PID Controllers for chassis movement
+ private final PIDController xController;
+ private final PIDController yController;
+ private final PIDController rotationController;
+
+ // If vision is enabled for drivetrain odometry updating
+ // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
+ // instead
+ private boolean visionEnabled = true;
+
+ // Disables vision for the first few seconds after deploying
+ private Timer visionEnableTimer = new Timer();
+
+ // If the robot should algin to the angle
+ private boolean isAlign = false;
+ // Angle to align to, can be null
+ private Double alignAngle = null;
+ // used for drift control
+ private double currentHeading = 0;
+ // used for drift control
+ private boolean drive_turning = false;
+
+ private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+
+ // The pose supplier to drive to
+ private Supplier<Pose2d> desiredPoSupplier = () -> null;
+
+ private SwerveModulePose modulePoses;
+
+ // The previous pose to reset to if the current pose gets too far off the field
+ private Pose2d prevPose = new Pose2d();
+
+ private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ ;
+
+ private boolean slipped = false;
+
+ private double previousAngularVelocity = 0;
+
+ private double centerOfMassHeight = 0;
+
+ private Rotation2d rawGyroRotation = new Rotation2d();
+
+ // for vision yaw correction
+ private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
+
+ private final Field2d field = new Field2d();
+
+ /** Creates a new Swerve Style Drivetrain. */
+ public Drivetrain(Vision vision, GyroIO gyroIO) {
+ this.vision = vision;
+
+ modules = new Module[4];
+ this.gyroIO = gyroIO;
+ ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
+
+ if (RobotBase.isReal()) {
+ Arrays.stream(constants)
+ .forEach(
+ moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new Module(moduleConstants);
+ });
+ } else {
+ Arrays.stream(constants)
+ .forEach(
+ moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
+ });
+ }
+
+ /*
+ * By pausing init for a second before setting module offsets, we avoid a bug
+ * with inverting motors.
+ * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
+ */
+ Timer.delay(1.0);
+ resetModulesToAbsolute();
+ gyroIO.updateInputs(gyroInputs);
+ poseEstimator =
+ new SwerveDrivePoseEstimator(
+ DriveConstants.KINEMATICS,
+ gyroInputs.yawPosition,
+ updateModulePositions(),
+ new Pose2d(),
+ // Defaults, except trust pigeon more
+ VecBuilder.fill(0.1, 0.1, 0),
+ VisionConstants.VISION_STD_DEVS);
+ poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
+
+ // initialize PID controllers
+ xController =
+ new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ yController =
+ new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
+ rotationController.enableContinuousInput(-Math.PI, Math.PI);
+ rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
+
+ PhoenixOdometryThread.getInstance().start();
+
+ modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
+
+ PathPlannerLogging.setLogActivePathCallback(
+ (activePath) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+ }
+ });
+ PathPlannerLogging.setLogTargetPoseCallback(
+ (targetPose) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+ }
+ });
+
+ // PPLibTelemetry.enableCompetitionMode();
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Field", field);
+ }
+
+ // addMusic();
+
+ }
+
+ public void setPose(Translation2d pose) {
+ poseEstimator.resetTranslation(pose);
+ }
+
+ public void addMusic() {
+ ArrayList<TalonFX> motors = new ArrayList<>();
+ for (Module m : modules) {
+ motors.add(m.getMotors()[0]);
+ motors.add(m.getMotors()[1]);
+ }
+
+ TalonFX[] f = new TalonFX[8];
+
+ SmartDashboard.putData("Chirp", new Music(motors.toArray(f)));
+ }
+
+ public void close() {
+ // close each of the modules
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].close();
+ }
+ }
+
+ @Override
+ public void periodic() {
+ odometryLock.lock(); // Prevents odometry updates while reading data
+ gyroIO.updateInputs(gyroInputs);
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ for (var module : modules) {
+ module.periodic();
+ }
+ odometryLock.unlock();
+ // Update odometry
+ double[] sampleTimestamps =
+ gyroInputs.odometryYawTimestamps; // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
+ for (int i = 0; i < modules.length; i++) {
+ positions[i] = modules[i].getOdometryPositions();
+ sampleCount = Math.min(sampleCount, positions[i].length);
+ }
+
+ // cap samples per cycle, more gives little benefit
+ final int MAX_SAMPLES_PER_CYCLE = 10;
+ if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
+ sampleCount = MAX_SAMPLES_PER_CYCLE;
+ }
+
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = positions[moduleIndex][i];
+ }
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.odometryYawPositions[i];
+ // Apply update
+ poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ }
+ updateOdometryVision();
+
+ field.setRobotPose(getPose());
+ }
+
+ // DRIVE
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward) in m/s
+ * @param ySpeed speed of the robot in the y direction (sideways) in m/s
+ * @param rot angular rate of the robot in rad/s
+ * @param fieldRelative whether the provided x and y speeds are relative to the field
+ * @param isOpenLoop whether to use velocity control for the drive motors
+ */
+ public void drive(
+ double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
+ // rot = headingControl(rot, xSpeed, ySpeed);
+ ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, isOpenLoop);
+ }
+
+ /**
+ * Drives the robot using the provided x speed, y speed, and positional heading.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward)
+ * @param ySpeed speed of the robot in the y direction (sideways)
+ * @param heading target heading of the robot in radians
+ * @param fieldRelative whether the provided x and y speeds are relative to the field
+ */
+ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
+ double rot = rotationController.calculate(getYaw().getRadians(), heading);
+ ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, false);
+ }
+
+ /**
+ * Runs the PID controllers with the provided x, y, and rot values. Then, calls {@link
+ * #drive(double, double, double, boolean, boolean)} using the PID outputs. This is based on the
+ * odometry of the chassis.
+ *
+ * @param x the position to move to in the x, in meters
+ * @param y the position to move to in the y, in meters
+ * @param rot the angle to move to, in radians
+ */
+ public void driveWithPID(double x, double y, double rot) {
+ Pose2d pose = getPose();
+ double xSpeed = xController.calculate(pose.getX(), x);
+ double ySpeed = yController.calculate(pose.getY(), y);
+ double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
+ drive(xSpeed, ySpeed, rotRadians, true, false);
+ }
+
+ /** Updates odometry using vision */
+ public void updateOdometryVision() {
+ // Start the timer if it hasn't started yet
+ visionEnableTimer.start();
+
+ // Update the swerve module poses
+ modulePoses.update();
+
+ if (modulePoses.slipped()) {
+ slipped = true;
+ }
+
+ Pose2d pose2 = getPose();
+
+ // Even if vision is disabled, it should still update inputs
+ // This prevents it from storing a lot of unread results, and it could be useful
+ // for replays
+ if (vision != null) {
+ vision.updateInputs();
+ }
+
+ if (VisionConstants.ENABLED) {
+ if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
+ vision.updateOdometry(
+ poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
+
+ if (vision.canSeeTag()) {
+ slipped = false;
+ modulePoses.reset();
+
+ double currentGyroYaw = gyroInputs.yawPosition.getRadians();
+
+ // to compare bias
+ ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
+
+ for (EstimatedRobotPose visionPose : visionPoses) {
+ if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
+ double visionYaw = visionPose.estimatedPose.getRotation().getZ();
+
+ // gets at vision timestamp, not current gyro yaw
+ double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
+
+ if (!Double.isNaN(gyroYawAtTimestamp)) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
+ Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
+ }
+ // use weighted observation
+ gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
+ }
+ }
+ }
+
+ // check if we have enough samples
+ if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
+ double fullBias = gyroBiasEstimator.getAndResetBias();
+ double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
+
+ if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
+ gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
+ }
+ }
+ }
+ }
+ }
+
+ Pose2d pose3 = getPose();
+
+ // Reset the pose to a position on the field if it is too far off the field
+ // This uses nearField() instead of onField() so we don't reset the odometry
+ // when the wheels slip near the edge of the field
+ // This is meant for poses that are caused by errors
+ if (!Vision.nearField(prevPose)) {
+ // If the pose at the beginning of the method is off the field, reset to a
+ // position in the middle of the field
+ // Use the rotation of the pose after updating odometry so the yaw is right
+ prevPose =
+ new Pose2d(
+ FieldConstants.field.getFieldLength() / 2,
+ FieldConstants.field.getFieldWidth() / 2,
+ pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose2)) {
+ // if the drivetrain pose is off the field, reset our odometry to the pose
+ // before(this is the right pose)
+ // Keep the rotation from pose2 so yaw is correct for driver
+ prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose3)) {
+ // if our vision+drivetrain odometry isn't near the field, reset our odometry to
+ // the pose before(this is the right pose)
+ resetOdometry(pose2);
+ prevPose = pose2;
+ } else {
+ // Set the previous pose to the current pose if we need to return to that
+ prevPose = pose3;
+ }
+
+ // if (Robot.isSimulation()) {
+ // pigeon.getSimState().addYaw(
+ // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
+ // * Constants.LOOP_TIME));
+ // }
+ }
+
+ /** Stops all swerve modules. */
+ public void stop() {
+ Arrays.stream(modules).forEach(Module::stop);
+ }
+
+ // for current limit setting (brownout protection)
+ public void applyNewModuleCurrents(
+ double steerCurrentStator,
+ double steerCurrentSupply,
+ double driveCurrentStator,
+ double driveCurrentSupply) {
+ for (Module module : modules) { // iterate over our modules
+ module.setNewCurrentLimit(
+ steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
+ }
+ }
+
+ public double getSubsystemStatorCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleStatorCurrent();
+ }
+ return sum;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleSupplyCurrent();
+ }
+ return sum;
+ }
+
+ /**
+ * Sets the desired states for all swerve modules.
+ *
+ * @param swerveModuleStates an array of module states to set swerve modules to. Order of the
+ * array matters here!
+ */
+ public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+ // makes sure speeds of modules don't exceed maximum allowed
+ SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+ }
+ }
+
+ /**
+ * Sets the chassis speeds of the robot.
+ *
+ * @param chassisSpeeds the target chassis speeds
+ * @param isOpenLoop if open loop control should be used for the drive velocity
+ */
+ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+
+ if (DriveConstants.USE_ACTUAL_SPEED) {
+ SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+ currentSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentState,
+ chassisSpeeds,
+ Constants.LOOP_TIME);
+ } else {
+ currentSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentSetpoint,
+ chassisSpeeds,
+ Constants.LOOP_TIME);
+ }
+
+ SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
+ setModuleStates(swerveModuleStates, isOpenLoop);
+ }
+
+ public void setDriveVoltages(Voltage voltage) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setDriveVoltage(voltage);
+ }
+ }
+
+ public void setAngleMotors(Rotation2d[] angles) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setAngle(angles[i]);
+ }
+ }
+
+ /**
+ * Returns the angular rate from the pigeon.
+ *
+ * @param id 0 for x, 1 for y, 2 for z
+ * @return the rate in rads/s from the pigeon
+ */
+ public double getAngularRate(int id) {
+ // double speed = 0;
+ // switch(id){
+ // case 0:
+ // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
+ // break;
+ // case 1:
+ // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
+ // break;
+ // case 2:
+ // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
+ // break;
+ // }
+ // outputs in deg/s, so convert to rad/s
+ return gyroInputs.yawVelocityRadPerSec;
+ }
+
+ /**
+ * Updates and returns the array of SwerveModulePositions, which store the distance travleled by
+ * the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ private SwerveModulePosition[] updateModulePositions() {
+ return modulePositions =
+ Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+ }
+
+ /**
+ * Gets an array of SwerveModulePositions, which store the distance travleled by the drive and the
+ * steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ public SwerveModulePosition[] getModulePositions() {
+ return modulePositions;
+ }
+
+ /**
+ * Enables or disables the state deadband for all swerve modules. The state deadband determines if
+ * the robot will stop drive and steer motors when inputted drive velocity is low. It should be
+ * enabled for all regular driving, to prevent releasing the controls from setting the angles.
+ */
+ public void setStateDeadband(boolean stateDeadBand) {
+ Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
+ }
+
+ public void setOptimized(boolean optimized) {
+ Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
+ }
+
+ public void setVisionEnabled(boolean enabled) {
+ visionEnabled = enabled;
+ }
+
+ public void setIsAlign(boolean isAlign) {
+ this.isAlign = isAlign;
+ }
+
+ public boolean getIsAlign() {
+ return isAlign;
+ }
+
+ /**
+ * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+ *
+ * @return ChassisSpeeds object This is often used as an input for other methods
+ */
+ public ChassisSpeeds getChassisSpeeds() {
+ return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+ }
+
+ /**
+ * Gets the state of each module
+ *
+ * @return An array of 4 SwerveModuleStates
+ */
+ public SwerveModuleState[] getModuleStates() {
+ return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+ }
+
+ public SwerveSetpoint getCurrSetpoint() {
+ return currentSetpoint;
+ }
+
+ /**
+ * @return the yaw of the robot, aka heading, the direction it is facing
+ */
+ public Rotation2d getYaw() {
+ return getPose().getRotation();
+ }
+
+ /**
+ * @return an array of modules
+ */
+ public Module[] getModules() {
+ return modules;
+ }
+
+ /**
+ * gets gyro yaw at a specific timestamp with interpolation this is used for
+ * timestamp-synchronized gyro/vision comparison.
+ *
+ * @param timestampSeconds the timestamp to get the gyro yaw at
+ * @return the gyro yaw in radians, or Double.NaN if no valid data
+ */
+ private double getGyroYawAtTimestamp(double timestampSeconds) {
+ return getPose().getRotation().getRadians();
+ }
+
+ /**
+ * Resets the yaw of the robot.
+ *
+ * @param rotation the new yaw angle as Rotation2d
+ */
+ public void setYaw(Rotation2d rotation) {
+ resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
+ }
+
+ /**
+ * Resets the odometry to the given pose.
+ *
+ * @param pose the pose to reset to.
+ */
+ public void resetOdometry(Pose2d pose) {
+ // NOTE: must use pigeon yaw for odometer!
+ currentHeading = pose.getRotation().getRadians();
+ poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
+ modulePoses.reset();
+ }
+
+ /**
+ * @return the pose of the robot as estimated by the odometry
+ */
+ @AutoLogOutput(key = "Odometry/Robot")
+ public Pose2d getPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ /**
+ * Sets the angle to align to
+ *
+ * @param newAngle The new angle in radians, can be set to null
+ */
+ public void setAlignAngle(Double newAngle) {
+ alignAngle = newAngle;
+ }
+
+ /**
+ * Returns whether or not the robot is at the input align angle
+ *
+ * @return true if it within tolerance the align angle, false otherwise
+ */
+ public boolean atAlignAngle() {
+ if (alignAngle == null) {
+ return false;
+ }
+ double diff = Math.abs(alignAngle - getYaw().getRadians());
+ return diff < DriveConstants.HEADING_TOLERANCE
+ || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
+ }
+
+ /**
+ * Gets the angle to align to
+ *
+ * @return The angle in radians
+ */
+ public double getAlignAngle() {
+ if (alignAngle != null) {
+ return alignAngle;
+ }
+ return 0;
+ }
+
+ /**
+ * Sets vision to only use certain April tags
+ *
+ * @param ids An array of the tags to only use
+ */
+ public void onlyUseTags(int[] ids) {
+ if (vision != null) {
+ vision.onlyUse(ids);
+ }
+ }
+
+ /**
+ * Returns if vision has seen an April tag in the last frame
+ *
+ * @return true if vision saw a tag last frame or if vision is disabled
+ */
+ public boolean canSeeTag() {
+ // if no vision system, then return true
+ if (vision == null) return true;
+
+ return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
+ }
+
+ /**
+ * Gets the pose at a previous time
+ *
+ * @param timestamp The timestamp of the pose to get
+ * @return The pose, null if there are no poses yet, or the current pose if timestamp < 0
+ */
+ public Pose2d getPoseAt(double timestamp) {
+ if (timestamp < 0) {
+ return getPose();
+ }
+ Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
+ if (pose.isPresent()) {
+ return pose.get();
+ } else {
+ return null;
+ }
+ }
+
+ /**
+ * Uses pigeon and rotational input to return a rotation that accounts for drift
+ *
+ * @return A rotation
+ */
+ public double headingControl(double rot, double xSpeed, double ySpeed) {
+ if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
+ && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
+ || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
+ drive_turning = true;
+ currentHeading = getYaw().getRadians();
+ } else {
+ drive_turning = false;
+ }
+ if (!drive_turning) {
+ rotationController.setSetpoint(currentHeading);
+ double output = rotationController.calculate(getYaw().getRadians());
+ rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ }
+ return rot;
+ }
+
+ /** Resets the swerve modules from the absolute encoders */
+ public void resetModulesToAbsolute() {
+ Arrays.stream(modules).forEach(Module::resetToAbsolute);
+ }
+
+ // getters for the PID Controllers
+ public PIDController getXController() {
+ return xController;
+ }
+
+ public PIDController getYController() {
+ return yController;
+ }
+
+ public PIDController getRotationController() {
+ return rotationController;
+ }
+
+ /**
+ * Set the desired pose to drive to This will enable driver assist to go to the pose
+ *
+ * @param supplier The supplier for the desired pose, use ()->null to not use a desired pose
+ */
+ public void setDesiredPose(Supplier<Pose2d> supplier) {
+ desiredPoSupplier = supplier;
+ }
+
+ /**
+ * Set the desired pose to drive to This will enable driver assist to go to the pose
+ *
+ * @param pose The Pose2d to drive to
+ */
+ public void setDesiredPose(Pose2d pose) {
+ setDesiredPose(() -> pose);
+ }
+
+ /**
+ * Gets the current desired pose, or null if there is no desired pose
+ *
+ * @return The Pose2d if it exists, null otherwise
+ */
+ public Pose2d getDesiredPose() {
+ return desiredPoSupplier.get();
+ }
+
+ public boolean atSetpoint() {
+ Pose2d pose = getDesiredPose();
+ return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
+ }
+
+ public SwerveModulePose getSwerveModulePose() {
+ return modulePoses;
+ }
+
+ public double getAcceleration() {
+ double accelX = gyroInputs.accelerationX;
+ double accelY = gyroInputs.accelerationY;
+
+ double angularVelocity = getAngularRate(3);
+ double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
+ previousAngularVelocity = angularVelocity;
+
+ double pigeonOffsetX = 0.082677;
+ double pigeonOffsetY = 0.030603444;
+
+ double totalX =
+ accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
+ double totalY =
+ accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
+
+ return Math.hypot(totalX, totalY);
+ }
+
+ @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
+ public boolean accelerationOverMax() {
+ return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
+ }
+
+ public void setCenterOfMass(double height) {
+ centerOfMassHeight = height;
+ }
+
+ public void alignWheels() {
+ SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
+ setModuleStates(new SwerveModuleState[] {state, state, state, state}, false);
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.StatusSignal;
+
+public interface GyroIO {
+ @AutoLog
+ public static class GyroIOInputs {
+ public boolean connected = false;
+ public Rotation2d yawPosition = new Rotation2d();
+ public double yawVelocityRadPerSec = 0.0;
+ public double accelerationX = 0.0;
+ public double accelerationY = 0.0;
+ public double[] odometryYawTimestamps = new double[] {};
+ public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+ }
+
+ public default void updateInputs(GyroIOInputs inputs) {}
+
+ /** returns the yaw status signal for time-synced odometry. */
+ public StatusSignal<?> getYawSignal();
+
+ /**
+ * set the yaw angle of the gyro.
+ *
+ * @param rotation the new yaw angle
+ */
+ public default void setYaw(Rotation2d rotation) {}
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.LinearAcceleration;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.PhoenixOdometryThread;
+
+/** IO implementation for Pigeon 2. */
+public class GyroIOPigeon2 implements GyroIO {
+ private final Pigeon2 pigeon = new Pigeon2(IdConstants.PIGEON, DriveConstants.PIGEON_CAN);
+ private final StatusSignal<Angle> yaw = pigeon.getYaw();
+ private final StatusSignal<LinearAcceleration> accelrationx = pigeon.getAccelerationX();
+ private final StatusSignal<LinearAcceleration> accelrationy = pigeon.getAccelerationY();
+ private final Queue<Double> yawPositionQueue;
+ private final Queue<Double> yawTimestampQueue;
+ private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+ private final Pigeon2Configuration config = new Pigeon2Configuration();
+
+ public GyroIOPigeon2() {
+ config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL;
+ pigeon.getConfigurator().apply(config);
+ pigeon.getConfigurator().setYaw(0.0);
+ yaw.setUpdateFrequency(250);
+ yawVelocity.setUpdateFrequency(50.0);
+ pigeon.optimizeBusUtilization();
+ yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+ }
+
+ @Override
+ public void updateInputs(GyroIOInputs inputs) {
+ inputs.connected =
+ BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy)
+ .equals(StatusCode.OK);
+ inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+ inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+ inputs.accelerationX = accelrationx.getValueAsDouble();
+ inputs.accelerationY = accelrationy.getValueAsDouble();
+
+ inputs.odometryYawTimestamps =
+ yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryYawPositions =
+ yawPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromDegrees(value))
+ .toArray(Rotation2d[]::new);
+ yawTimestampQueue.clear();
+ yawPositionQueue.clear();
+ }
+
+ @Override
+ public StatusSignal<Angle> getYawSignal() {
+ return yaw;
+ }
+
+ @Override
+ public void setYaw(Rotation2d rotation) {
+ pigeon.getConfigurator().setYaw(rotation.getDegrees());
+ }
+}
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Angle;
+import frc.robot.constants.Constants;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.Current;
+import org.wpilib.units.measure.Voltage;
+import org.wpilib.util.Alert;
+import org.wpilib.util.Alert.AlertType;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.constants.swerve.ModuleType;
+import frc.robot.util.PhoenixOdometryThread;
+import lib.CTREModuleState;
+
+public class Module implements ModuleIO {
+ private final ModuleType type;
+
+ // Degrees
+ private final double angleOffset;
+
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ private boolean optimizeStates = true;
+
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
+
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
+
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
+
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest =
+ new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
+
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveDisconnectedAlert =
+ new Alert(
+ "Disconnected drive motor on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
+ turnDisconnectedAlert =
+ new Alert(
+ "Disconnected turn motor on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
+ turnEncoderDisconnectedAlert =
+ new Alert(
+ "Disconnected turn encoder on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus =
+ BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus =
+ BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(
+ drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsToRadians(
+ driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition =
+ Rotation2d.fromRotations(
+ turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsToRadians(
+ turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset =
+ Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
+
+ // Update odometry inputs
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
+ inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
+ inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters =
+ inputs.odometryDrivePositionsRad[i]
+ / DriveConstants.DRIVE_GEAR_RATIO
+ * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle =
+ inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Angle " + moduleConstants.ordinal(),
+ MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState =
+ optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ } else {
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if (desiredState == null) {
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity =
+ desiredState.speedMetersPerSecond
+ / DriveConstants.WHEEL_RADIUS
+ / 2
+ / Math.PI
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+ }
+
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(velocityRequest.withVelocity(velocity).withFeedForward(feedforward));
+ }
+ }
+
+ private void setAngle() {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
+ if (stateDeadband
+ && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
+ }
+ if (desiredState == null) {
+ return;
+ }
+ angleMotor.setControl(
+ new PositionDutyCycle(
+ desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage) {
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+
+ public void setAngle(Rotation2d angle) {
+ angleMotor.setControl(
+ new PositionDutyCycle(
+ angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator()
+ .apply(
+ new MagnetSensorConfigs()
+ .withAbsoluteSensorDiscontinuityPoint(1)
+ .withSensorDirection(
+ DriveConstants.MODULE_CONSTANTS.canCoderInvert
+ ? SensorDirectionValue.Clockwise_Positive
+ : SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor
+ .getConfigurator()
+ .apply(
+ new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor
+ .getConfigurator()
+ .apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec
+ * 60
+ / DriveConstants.MODULE_CONSTANTS.driveGearRatio
+ / 2
+ / Math.PI;
+ }
+
+ public double getDriveVoltage() {
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent() {
+ return inputs.driveCurrentAmps;
+ }
+
+ public double getModuleStatorCurrent() {
+ return inputs.steerStator + inputs.driveStator;
+ }
+
+ public double getModuleSupplyCurrent() {
+ return inputs.steerSupply + inputs.driveSupply;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(
+ double currentSteerStator,
+ double currentSteerSupply,
+ double currentDriveStator,
+ double currentDriveSupply) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = true;
+ driveConfig.StatorCurrentLimitEnable = true;
+ driveConfig.SupplyCurrentLimit = currentDriveSupply;
+ driveConfig.StatorCurrentLimit = currentDriveStator;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity =
+ DriveConstants.MAX_SPEED
+ / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration =
+ DriveConstants.MAX_DRIVE_ACCEL
+ / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP =
+ moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD =
+ moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor
+ .getConfigurator()
+ .apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor
+ .getConfigurator()
+ .apply(
+ new OpenLoopRampsConfigs()
+ .withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor
+ .getConfigurator()
+ .apply(
+ new ClosedLoopRampsConfigs()
+ .withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType() {
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /** returns the turn absolute position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[] {angleMotor, driveMotor};
+ }
+}
--- /dev/null
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ModuleIO {
+ @AutoLog
+ public static class ModuleIOInputs {
+ public boolean driveConnected = false;
+ public double drivePositionRad = 0.0;
+ public double driveVelocityRadPerSec = 0.0;
+ public double driveAppliedVolts = 0.0;
+ public double driveCurrentAmps = 0.0;
+
+ public boolean turnConnected = false;
+ public boolean turnEncoderConnected = false;
+ public Rotation2d turnAbsolutePosition = new Rotation2d();
+ public Rotation2d turnPosition = new Rotation2d();
+ public double turnVelocityRadPerSec = 0.0;
+ public double turnAppliedVolts = 0.0;
+ public double turnCurrentAmps = 0.0;
+
+ public double[] odometryTimestamps = new double[] {};
+ public double[] odometryDrivePositionsRad = new double[] {};
+ public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+
+ public double encoderOffset = 0.0;
+
+ // drivetrain is scary. I'm adding my own seperate logging
+ public double driveStator = 0.0;
+ public double driveSupply = 0.0;
+ public double steerStator = 0.0;
+ public double steerSupply = 0.0;
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs() {}
+}
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.system.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import lib.CTREModuleState;
+
+/**
+ * Swerve module for drivetrain to be used inside of simulation. TODO: improve this simulation to be
+ * more realistic
+ */
+public class ModuleSim extends Module {
+
+ private double currentSteerPositionRad = 0;
+ private double currentDrivePositionMeters = 0;
+ private double currentSpeed = 0;
+
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ public ModuleSim(ModuleConstants ignored) {
+ super(ignored);
+ }
+
+ /** Updates the simulation */
+ @Override
+ public void periodic() {
+ currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME;
+ super.periodic();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Update drive inputs
+ inputs.driveConnected = true;
+ inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS;
+ inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS;
+ inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE;
+ inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this
+
+ // Update turn inputs
+ inputs.turnConnected = true;
+ inputs.turnEncoderConnected = true;
+ inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnPosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport
+ inputs.turnAppliedVolts = 0;
+ inputs.turnCurrentAmps = 0;
+
+ // Update odometry inputs
+ // Simulate as only getting one value per frame
+ inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+ inputs.odometryDrivePositionsRad =
+ new double[] {inputs.drivePositionRad * DriveConstants.DRIVE_GEAR_RATIO};
+ inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param desiredState Desired state with speed and angle.
+ * @param isOpenLoop whether to use closed/open loop control for drive velocity
+ */
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // If the module isn't moving, don't rotate it
+ if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
+ currentSpeed = 0;
+ return;
+ }
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ desiredState =
+ CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad));
+ }
+
+ currentSpeed = desiredState.speedMetersPerSecond;
+ currentSteerPositionRad = desiredState.angle.getRadians();
+ }
+
+ public void resetToAbsolute() {
+ // does nothing when robot does not have a swerve drivetrain
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Sets current speed to zero */
+ public void stop() {
+ currentSpeed = 0;
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(currentSpeed, getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ currentDrivePositionMeters, new Rotation2d(currentSteerPositionRad));
+ }
+
+ /** Gets the simulated angle of the module. */
+ public Rotation2d getAngle() {
+ return new Rotation2d(currentSteerPositionRad);
+ }
+
+ /** Sets state deadband */
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public TalonFX getDriveMotor() {
+ return null;
+ }
+
+ public double getDriveVoltage() {
+ return 0;
+ }
+
+ public double getDriveStatorCurrent() {
+ return 0;
+ }
+
+ public double getSteerVelocity() {
+ return 0;
+ }
+
+ public double getDriveVelocity() {
+ return 0;
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N2;
+import org.wpilib.math.system.NumericalIntegration;
+import org.wpilib.math.system.plant.DCMotor;
+import org.wpilib.simulation.ElevatorSim;
+
+/** Exactly the same as ElevatorSim, except it can be angled and have a constant force spring */
+public class AngledElevatorSim extends ElevatorSim {
+ private double angle;
+ private boolean simulateGravity;
+ private double minHeight;
+ private double maxHeight;
+ private double springAccel;
+
+ /**
+ * Creates a simulated angled elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param angleRads The angle of the elevator from vertical in radians.
+ * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public AngledElevatorSim(
+ DCMotor gearbox,
+ double gearing,
+ double carriageMassKg,
+ double drumRadiusMeters,
+ double minHeightMeters,
+ double maxHeightMeters,
+ boolean simulateGravity,
+ double startingHeightMeters,
+ double angleRads,
+ double springForceNewtons,
+ double... measurementStdDevs) {
+ super(
+ gearbox,
+ gearing,
+ carriageMassKg,
+ drumRadiusMeters,
+ minHeightMeters,
+ maxHeightMeters,
+ simulateGravity,
+ startingHeightMeters,
+ measurementStdDevs);
+ angle = angleRads;
+ this.simulateGravity = simulateGravity;
+ minHeight = minHeightMeters;
+ maxHeight = maxHeightMeters;
+ springAccel = springForceNewtons / carriageMassKg;
+ }
+
+ // Copied from ElevatorSim with one difference
+ /**
+ * Creates a simulated elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ @Override
+ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+ // Calculate updated x-hat from Runge-Kutta.
+ var updatedXhat =
+ NumericalIntegration.rkdp(
+ (x, _u) -> {
+ Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+ if (simulateGravity) {
+ // This is the only line that is different
+ xdot = xdot.plus(VecBuilder.fill(0, springAccel - 9.8 * Math.cos(angle)));
+ }
+ return xdot;
+ },
+ currentXhat,
+ u,
+ dtSeconds);
+
+ // We check for collisions after updating x-hat.
+ if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(minHeight, 0);
+ }
+ if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(maxHeight, 0);
+ }
+ return updatedXhat;
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+/**
+ * Automatically generated file containing build version information.
+ */
+public final class BuildData {
+ public static final String MAVEN_GROUP = "";
+ public static final String MAVEN_NAME = "FRC2027";
+ public static final String VERSION = "unspecified";
+ public static final int GIT_REVISION = 23;
+ public static final String GIT_SHA = "f59c39e0630109646b452869f6842ebcc9f5d923";
+ public static final String GIT_DATE = "2026-08-22T16:10:21Z";
+ public static final String GIT_BRANCH = "main";
+ public static final String BUILD_DATE = "2026-08-23T16:49:48Z";
+ public static final long BUILD_UNIX_TIME = 1787503788150L;
+ public static final int DIRTY = 0;
+
+ private BuildData(){}
+}
--- /dev/null
+package frc.robot.util;
+
+public final class ChineseRemainderTheorem {
+
+ private ChineseRemainderTheorem() {}
+
+ /**
+ * Computes x such that: x ≡ a (mod n1) x ≡ b (mod n2)
+ *
+ * <p>n1 and n2 MUST be coprime.
+ *
+ * <p>Returns x in range [0, n1*n2)
+ */
+ public static int solve(int a, int n1, int b, int n2) {
+ if (gcd(n1, n2) != 1) {
+ throw new IllegalArgumentException("Moduli must be coprime for CRT.");
+ }
+
+ int N = n1 * n2;
+
+ int invN1modN2 = modInverse(n1, n2);
+ int invN2modN1 = modInverse(n2, n1);
+
+ int result = (a * n2 * invN2modN1 + b * n1 * invN1modN2) % N;
+
+ return (result + N) % N;
+ }
+
+ private static int modInverse(int a, int mod) {
+ a = ((a % mod) + mod) % mod;
+
+ for (int x = 1; x < mod; x++) {
+ if ((a * x) % mod == 1) {
+ return x;
+ }
+ }
+ throw new IllegalStateException("No modular inverse exists.");
+ }
+
+ private static int gcd(int a, int b) {
+ while (b != 0) {
+ int t = b;
+ b = a % b;
+ a = t;
+ }
+ return Math.abs(a);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N2;
+import org.wpilib.math.system.LinearSystem;
+import org.wpilib.math.system.NumericalIntegration;
+import org.wpilib.math.system.plant.DCMotor;
+import org.wpilib.math.system.plant.LinearSystemId;
+import org.wpilib.simulation.SingleJointedArmSim;
+
+/**
+ * Similar to SingleJointedArmSim, except it simulates an upward normal force on the end of the arm
+ * equal to the weight of the robot Use setIsClimbing() to change whether this normal force should
+ * be simulated
+ */
+public class ClimbArmSim extends SingleJointedArmSim {
+ private boolean simulateGravity;
+ private double armLenMeters;
+ private double minAngle;
+ private double maxAngle;
+ private double mass;
+ private double momentOfInertia;
+ private boolean isClimbing;
+
+ /**
+ * Creates a simulated arm mechanism.
+ *
+ * @param plant The linear system that represents the arm. This system can be created with {@link
+ * org.wpilib.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor,
+ * double, double)}.
+ * @param gearbox The type of and number of motors in the arm gearbox.
+ * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+ * @param armLengthMeters The length of the arm.
+ * @param minAngleRads The minimum angle that the arm is capable of.
+ * @param maxAngleRads The maximum angle that the arm is capable of.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingAngleRads The initial position of the Arm simulation in radians.
+ * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public ClimbArmSim(
+ LinearSystem<N2, N1, N2> plant,
+ DCMotor gearbox,
+ double gearing,
+ double armLengthMeters,
+ double minAngleRads,
+ double maxAngleRads,
+ boolean simulateGravity,
+ double startingAngleRads,
+ double robotMasKilograms,
+ double armMassKilograms,
+ double... measurementStdDevs) {
+ super(
+ plant,
+ gearbox,
+ gearing,
+ armLengthMeters,
+ minAngleRads,
+ maxAngleRads,
+ simulateGravity,
+ startingAngleRads,
+ measurementStdDevs);
+ armLenMeters = armLengthMeters;
+ minAngle = minAngleRads;
+ maxAngle = maxAngleRads;
+ this.simulateGravity = simulateGravity;
+ mass = robotMasKilograms;
+ momentOfInertia = 1.0 / 3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
+ isClimbing = false;
+ }
+
+ /**
+ * Creates a simulated arm mechanism.
+ *
+ * @param gearbox The type of and number of motors in the arm gearbox.
+ * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+ * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
+ * @param armLengthMeters The length of the arm.
+ * @param minAngleRads The minimum angle that the arm is capable of.
+ * @param maxAngleRads The maximum angle that the arm is capable of.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingAngleRads The initial position of the Arm simulation in radians.
+ * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public ClimbArmSim(
+ DCMotor gearbox,
+ double gearing,
+ double jKgMetersSquared,
+ double armLengthMeters,
+ double minAngleRads,
+ double maxAngleRads,
+ boolean simulateGravity,
+ double startingAngleRads,
+ double robotMassKilograms,
+ double... measurementStdDevs) {
+ this(
+ LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+ gearbox,
+ gearing,
+ armLengthMeters,
+ minAngleRads,
+ maxAngleRads,
+ simulateGravity,
+ startingAngleRads,
+ robotMassKilograms,
+ 1,
+ measurementStdDevs);
+ momentOfInertia = jKgMetersSquared;
+ }
+
+ public void setIsClimbing(boolean climbing) {
+ isClimbing = climbing;
+ }
+
+ /**
+ * Updates the state of the arm.
+ *
+ * @param currentXhat The current state estimate.
+ * @param u The system inputs (voltage).
+ * @param dtSeconds The time difference between controller updates.
+ */
+ @Override
+ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+ // The torque on the arm is given by Ï„ = Fâ‹…r, where F is the force applied by
+ // gravity and r the distance from pivot to center of mass. Recall from
+ // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
+ // torque on the arm, J is the mass-moment of inertia about the pivot axis,
+ // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J
+ //
+ // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal:
+ //
+ // α = (m⋅g⋅cos(θ))⋅r/J
+ //
+ // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the
+ // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a
+ // rod rotating about it's end, where L is the overall rod length. The mass
+ // distribution is assumed to be uniform. Substitute r=L/2 to find:
+ //
+ // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²)
+ // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²)
+ // α = 3/2⋅g⋅cos(θ)/L
+ //
+ // Adding the torque from the robot weight, which is in the opposite direction as the arm's mass
+ // α = 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J
+ //
+ // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu
+ //
+ // f(x, u) = Ax + Bu + [0 α]ᵀ
+ // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J]ᵀ
+
+ Matrix<N2, N1> updatedXhat =
+ NumericalIntegration.rkdp(
+ (Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
+ Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+ if (simulateGravity) {
+ double alphaGrav =
+ 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / armLenMeters
+ + (isClimbing
+ ? mass * 9.8 * Math.cos(x.get(0, 0)) * armLenMeters / momentOfInertia
+ : 0);
+ xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
+ }
+ return xdot;
+ },
+ currentXhat,
+ u,
+ dtSeconds);
+
+ // We check for collision after updating xhat
+ if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(minAngle, 0);
+ }
+ if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(maxAngle, 0);
+ }
+ return updatedXhat;
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import frc.robot.constants.FieldConstants;
+
+public class ConversionUtils {
+
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double degreesToCANcoder(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Radians of Rotation of Mechanism
+ */
+ public static double CANcoderToRadians(double positionCounts, double gearRatio) {
+ return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio));
+ }
+
+ /**
+ * @param radians Radians of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double radiansToCANcoder(double radians, double gearRatio) {
+ return degreesToCANcoder(Math.toDegrees(radians), gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double falconToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Falcon Position Counts
+ */
+ public static double degreesToFalcon(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param velocityCounts Falcon Velocity Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double falconToRPM(double velocityCounts, double gearRatio) {
+ double motorRPM = velocityCounts * (600.0 / 2048.0);
+ return motorRPM / gearRatio;
+ }
+
+ /**
+ * @param RPM RPM of mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double RPMToFalcon(double RPM, double gearRatio) {
+ double motorRPM = RPM * gearRatio;
+ return motorRPM * (2048.0 / 600.0);
+ }
+
+ /**
+ * @param velocitycounts Falcon Velocity Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
+ double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+ return (wheelRPM * circumference) / 60;
+ }
+
+ /**
+ * @param velocity Velocity MPS
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double MPSToFalcon(double velocity, double circumference, double gearRatio) {
+ double wheelRPM = ((velocity * 60) / circumference);
+ return RPMToFalcon(wheelRPM, gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Meters
+ */
+ public static double falconToMeters(
+ double positionCounts, double circumference, double gearRatio) {
+ return positionCounts * (circumference / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param meters Meters
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Falcon Position Counts
+ */
+ public static double MetersToFalcon(double meters, double circumference, double gearRatio) {
+ return meters / (circumference / (gearRatio * 2048.0));
+ }
+
+ /**
+ * Converts between an absolute coordinate system and the pathplanner coordinate system.
+ *
+ * <p>Absolute coordinate system always has the origin right of the blue driver station from blue
+ * driver perspective, bottom left if looking down at the field. Positive X goes toward red
+ * alliance (forward from blue driver perspective) and positive Y toward red loading zone (left
+ * from blue driver perspective). The Pathplanner coordinate system has the coordinate system
+ * rotated such that the origin starts right of the current driver station.
+ *
+ * <p>The transformation is self-inverse, so there is no second function to convert back.
+ *
+ * @param pose pose to convert
+ * @param alliance alliance PathPlanner is using for their origin
+ * @return converted pose
+ */
+ public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
+ if (alliance == Alliance.Red) {
+ return pose.relativeTo(
+ new Pose2d(
+ FieldConstants.field.getFieldLength(),
+ FieldConstants.field.getFieldWidth(),
+ new Rotation2d(Math.PI)));
+ }
+ return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
+ }
+}
--- /dev/null
+// 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.util;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.util.WPIUtilJNI;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
+ * org.wpilib.math.trajectory.TrapezoidProfile} instead. Edited by 972 to be "dynamic", that is,
+ * the slew rate can be modified on the fly. Additionally, it can be set to be continuous on a
+ * range, useful for angles.
+ */
+public class DynamicSlewRateLimiter {
+ private double positiveRateLimit;
+ private double negativeRateLimit;
+ private double prevVal;
+ private double prevTime;
+
+ private boolean continuous = false;
+ private double lowerContinuousLimit = -1;
+ private double upperContinuousLimit = 1;
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and
+ * initial value.
+ *
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @param initialValue The initial value of the input.
+ */
+ public DynamicSlewRateLimiter(
+ double positiveRateLimit, double negativeRateLimit, double initialValue) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ prevVal = initialValue;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit
+ * of -rateLimit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ * @param initialValue The initial value of the input.
+ */
+ @Deprecated(since = "2023", forRemoval = true)
+ public DynamicSlewRateLimiter(double rateLimit, double initialValue) {
+ this(rateLimit, -rateLimit, initialValue);
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+ * -rateLimit.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ */
+ public DynamicSlewRateLimiter(double rateLimit) {
+ this(rateLimit, -rateLimit, 0);
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input) {
+ double currentTime = WPIUtilJNI.now() * 1e-6;
+ double elapsedTime = currentTime - prevTime;
+ prevTime = currentTime;
+
+ double change =
+ MathUtil.clamp(
+ input - prevVal, negativeRateLimit * elapsedTime, positiveRateLimit * elapsedTime);
+
+ if (continuous) {
+ change =
+ MathUtil.clamp(
+ MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit),
+ negativeRateLimit * elapsedTime,
+ positiveRateLimit * elapsedTime);
+
+ prevVal += change;
+
+ // Extra check to make sure it is within the limits, probably unnecessary
+ prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit);
+ } else {
+ prevVal += change;
+ }
+
+ return prevVal;
+ }
+
+ /**
+ * Sets a new slewrate and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param rateLimit The new rate-of-change limit, in units per second.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double rateLimit) {
+ setRateLimit(rateLimit);
+ return calculate(input);
+ }
+
+ /**
+ * Sets new slew rates and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double positiveRateLimit, double negativeRateLimit) {
+ setRateLimit(positiveRateLimit, negativeRateLimit);
+ return calculate(input);
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ public void reset(double value) {
+ prevVal = value;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * set positive rate limit
+ *
+ * @param positiveRateLimit new positive rate limit
+ */
+ public void setPositiveRateLimit(double positiveRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ }
+
+ /**
+ * set negative rate limit
+ *
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setNegativeRateLimit(double negativeRateLimit) {
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param rateLimit new rate limits
+ */
+ public void setRateLimit(double rateLimit) {
+ positiveRateLimit = rateLimit;
+ negativeRateLimit = -rateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param positiveRateLimit new positive rate limit
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setRateLimit(double positiveRateLimit, double negativeRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets Continuous Limits
+ *
+ * @param lowerContinuousLimit Lower Continuous Limit
+ * @param upperContinuousLimit Upper Continuous Limit
+ */
+ public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) {
+ this.lowerContinuousLimit = lowerContinuousLimit;
+ this.upperContinuousLimit = upperContinuousLimit;
+ }
+
+ /**
+ * Enables or disables continuous WARNING: Continuous doesn't work properly with non-symmetrical
+ * rate limits
+ *
+ * @param continuous is continuous enabled
+ */
+ public void enableContinuous(boolean continuous) {
+ this.continuous = continuous;
+ }
+}
--- /dev/null
+// Copyright (c) 2023-2026 Gold87 and other Elastic contributors
+// This software can be modified and/or shared under the terms
+// defined by the Elastic license:
+// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
+
+package frc.robot.util;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.networktables.StringPublisher;
+import org.wpilib.networktables.StringTopic;
+
+public final class Elastic {
+ private static final StringTopic notificationTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
+ private static final StringPublisher notificationPublisher =
+ notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
+ private static final StringTopic selectedTabTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
+ private static final StringPublisher selectedTabPublisher =
+ selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
+ private static final ObjectMapper objectMapper = new ObjectMapper();
+
+ /**
+ * Represents the possible levels of notifications for the Elastic dashboard. These levels are
+ * used to indicate the severity or type of notification.
+ */
+ public enum NotificationLevel {
+ /** Informational Message */
+ INFO,
+ /** Warning message */
+ WARNING,
+ /** Error message */
+ ERROR
+ }
+
+ /**
+ * Sends a notification to the Elastic dashboard. The notification is serialized as a JSON string
+ * before being published.
+ *
+ * @param notification the {@link Notification} object containing notification details
+ */
+ public static void sendNotification(Notification notification) {
+ try {
+ notificationPublisher.set(objectMapper.writeValueAsString(notification));
+ } catch (JsonProcessingException e) {
+ e.printStackTrace();
+ }
+ }
+
+ /**
+ * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
+ * have no effect on the widgets or tabs in view.
+ *
+ * <p>If the given name is a number, Elastic will select the tab whose index equals the number
+ * provided.
+ *
+ * @param tabName the name of the tab to select
+ */
+ public static void selectTab(String tabName) {
+ selectedTabPublisher.set(tabName);
+ }
+
+ /**
+ * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
+ * the number of tabs, this will have no effect.
+ *
+ * @param tabIndex the index of the tab to select.
+ */
+ public static void selectTab(int tabIndex) {
+ selectTab(Integer.toString(tabIndex));
+ }
+
+ /**
+ * Represents a notification object to be sent to the Elastic dashboard. This object holds
+ * properties such as level, title, description, display time, and dimensions to control how the
+ * notification is displayed on the dashboard.
+ */
+ public static class Notification {
+ @JsonProperty("level")
+ private NotificationLevel level;
+
+ @JsonProperty("title")
+ private String title;
+
+ @JsonProperty("description")
+ private String description;
+
+ @JsonProperty("displayTime")
+ private int displayTimeMillis;
+
+ @JsonProperty("width")
+ private double width;
+
+ @JsonProperty("height")
+ private double height;
+
+ /**
+ * Creates a new Notification with all default parameters. This constructor is intended to be
+ * used with the chainable decorator methods
+ *
+ * <p>Title and description fields are empty.
+ */
+ public Notification() {
+ this(NotificationLevel.INFO, "", "");
+ }
+
+ /**
+ * Creates a new Notification with all properties specified.
+ *
+ * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the time in milliseconds for which the notification is displayed
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level,
+ String title,
+ String description,
+ int displayTimeMillis,
+ double width,
+ double height) {
+ this.level = level;
+ this.title = title;
+ this.displayTimeMillis = displayTimeMillis;
+ this.description = description;
+ this.height = height;
+ this.width = width;
+ }
+
+ /**
+ * Creates a new Notification with default display time and dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ */
+ public Notification(NotificationLevel level, String title, String description) {
+ this(level, title, description, 3000, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with a specified display time and default dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the display time in milliseconds
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, int displayTimeMillis) {
+ this(level, title, description, displayTimeMillis, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with specified dimensions and default display time. If the height
+ * is below zero, it is automatically inferred based on screen size.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, double width, double height) {
+ this(level, title, description, 3000, width, height);
+ }
+
+ /**
+ * Updates the level of this notification
+ *
+ * @param level the level to set the notification to
+ */
+ public void setLevel(NotificationLevel level) {
+ this.level = level;
+ }
+
+ /**
+ * @return the level of this notification
+ */
+ public NotificationLevel getLevel() {
+ return level;
+ }
+
+ /**
+ * Updates the title of this notification
+ *
+ * @param title the title to set the notification to
+ */
+ public void setTitle(String title) {
+ this.title = title;
+ }
+
+ /**
+ * Gets the title of this notification
+ *
+ * @return the title of this notification
+ */
+ public String getTitle() {
+ return title;
+ }
+
+ /**
+ * Updates the description of this notification
+ *
+ * @param description the description to set the notification to
+ */
+ public void setDescription(String description) {
+ this.description = description;
+ }
+
+ public String getDescription() {
+ return description;
+ }
+
+ /**
+ * Updates the display time of the notification
+ *
+ * @param seconds the number of seconds to display the notification for
+ */
+ public void setDisplayTimeSeconds(double seconds) {
+ setDisplayTimeMillis((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Updates the display time of the notification in milliseconds
+ *
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ */
+ public void setDisplayTimeMillis(int displayTimeMillis) {
+ this.displayTimeMillis = displayTimeMillis;
+ }
+
+ /**
+ * Gets the display time of the notification in milliseconds
+ *
+ * @return the number of milliseconds the notification is displayed for
+ */
+ public int getDisplayTimeMillis() {
+ return displayTimeMillis;
+ }
+
+ /**
+ * Updates the width of the notification
+ *
+ * @param width the width to set the notification to
+ */
+ public void setWidth(double width) {
+ this.width = width;
+ }
+
+ /**
+ * Gets the width of the notification
+ *
+ * @return the width of the notification
+ */
+ public double getWidth() {
+ return width;
+ }
+
+ /**
+ * Updates the height of the notification
+ *
+ * <p>If the height is set to -1, the height will be determined automatically by the dashboard
+ *
+ * @param height the height to set the notification to
+ */
+ public void setHeight(double height) {
+ this.height = height;
+ }
+
+ /**
+ * Gets the height of the notification
+ *
+ * @return the height of the notification
+ */
+ public double getHeight() {
+ return height;
+ }
+
+ /**
+ * Modifies the notification's level and returns itself to allow for method chaining
+ *
+ * @param level the level to set the notification to
+ * @return the current notification
+ */
+ public Notification withLevel(NotificationLevel level) {
+ this.level = level;
+ return this;
+ }
+
+ /**
+ * Modifies the notification's title and returns itself to allow for method chaining
+ *
+ * @param title the title to set the notification to
+ * @return the current notification
+ */
+ public Notification withTitle(String title) {
+ setTitle(title);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's description and returns itself to allow for method chaining
+ *
+ * @param description the description to set the notification to
+ * @return the current notification
+ */
+ public Notification withDescription(String description) {
+ setDescription(description);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param seconds the number of seconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplaySeconds(double seconds) {
+ return withDisplayMilliseconds((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplayMilliseconds(int displayTimeMillis) {
+ setDisplayTimeMillis(displayTimeMillis);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's width and returns itself to allow for method chaining
+ *
+ * @param width the width to set the notification to
+ * @return the current notification
+ */
+ public Notification withWidth(double width) {
+ setWidth(width);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ * @param height the height to set the notification to
+ * @return the current notification
+ */
+ public Notification withHeight(double height) {
+ setHeight(height);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ * <p>This will set the height to -1 to have it automatically determined by the dashboard
+ *
+ * @return the current notification
+ */
+ public Notification withAutomaticHeight() {
+ setHeight(-1);
+ return this;
+ }
+
+ /**
+ * Modifies the notification to disable the auto dismiss behavior
+ *
+ * <p>This sets the display time to 0 milliseconds
+ *
+ * <p>The auto dismiss behavior can be re-enabled by setting the display time to a number
+ * greater than 0
+ *
+ * @return the current notification
+ */
+ public Notification withNoAutoDismiss() {
+ setDisplayTimeMillis(0);
+ return this;
+ }
+ }
+}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import org.wpilib.math.geometry.Twist2d;
+
+public class EqualsUtil {
+ public static boolean epsilonEquals(double a, double b, double epsilon) {
+ return (a - epsilon <= b) && (a + epsilon >= b);
+ }
+
+ public static boolean epsilonEquals(double a, double b) {
+ return epsilonEquals(a, b, 1e-9);
+ }
+
+ /** Extension methods for wpi geometry objects */
+ public static class GeomExtensions {
+ public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
+ return EqualsUtil.epsilonEquals(twist.dx, other.dx)
+ && EqualsUtil.epsilonEquals(twist.dy, other.dy)
+ && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
+ }
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+
+import java.util.LinkedList;
+import java.util.List;
+
+/**
+ * A class for storing and processing feedforward characterization data. Used in automatic
+ * feedforward characterization.
+ */
+public class FeedForwardCharacterizationData {
+ private PolynomialRegression regression;
+ private final List<Double> velocityData = new LinkedList<>();
+ private final List<Double> voltageData = new LinkedList<>();
+
+ /**
+ * Adds a data point to the data set.
+ *
+ * @param velocity the velocity of the motor
+ * @param voltage the voltage applied to the motor
+ */
+ public void add(double velocity, double voltage) {
+ if (Math.abs(velocity) > 1E-4) {
+ velocityData.add(Math.abs(velocity));
+ voltageData.add(Math.abs(voltage));
+ }
+ }
+
+ /**
+ * Processes the data set using {@link PolynomialRegression}
+ *
+ * @see PolynomialRegression
+ */
+ public void process() {
+ // creates a new process polynomial regression to get calculated values
+ regression =
+ new PolynomialRegression(
+ velocityData.stream().mapToDouble(Double::doubleValue).toArray(),
+ voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
+ 1);
+ }
+
+ /**
+ * Gets the static voltage of the motor.
+ *
+ * @return the static voltage of the motor
+ */
+ public double getStatic() {
+ // gets y-intercept
+ return regression.beta(0);
+ }
+
+ /**
+ * Gets the velocity of the motor.
+ *
+ * @return the velocity of the motor
+ */
+ public double getVelocity() {
+ // gets a slope of regression line
+ return regression.beta(1);
+ }
+
+ /**
+ * Gets the variance of the data set.
+ *
+ * @return the variance of the data set
+ */
+ public double getVariance() {
+ // gets variance of data set
+ return regression.R2();
+ }
+}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform2d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Twist2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
+public class GeomUtil {
+ /**
+ * Creates a pure translating transform
+ *
+ * @param translation The translation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Translation2d translation) {
+ return new Transform2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure translating transform
+ *
+ * @param x The x coordinate of the translation
+ * @param y The y coordinate of the translation
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(double x, double y) {
+ return new Transform2d(x, y, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotating transform
+ *
+ * @param rotation The rotation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Rotation2d rotation) {
+ return new Transform2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Pose2d pose) {
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ public static Pose2d inverse(Pose2d pose) {
+ Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+ return new Pose2d(
+ pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+ }
+
+ /**
+ * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Transform2d transform) {
+ return new Pose2d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Creates a pure translated pose
+ *
+ * @param translation The translation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Translation2d translation) {
+ return new Pose2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotated pose
+ *
+ * @param rotation The rotation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Rotation2d rotation) {
+ return new Pose2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Multiplies a twist by a scaling factor
+ *
+ * @param twist The twist to multiply
+ * @param factor The scaling factor for the twist components
+ * @return The new twist
+ */
+ public static Twist2d multiply(Twist2d twist, double factor) {
+ return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+ }
+
+ /**
+ * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform3d toTransform3d(Pose3d pose) {
+ return new Transform3d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose3d toPose3d(Transform3d transform) {
+ return new Pose3d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+ *
+ * @param speeds The original translation
+ * @return The resulting translation
+ */
+ public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+ return new Twist2d(
+ speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different translation value.
+ *
+ * @param pose The original pose
+ * @param translation The new translation to use
+ * @return The new pose with the new translation and original rotation
+ */
+ public static Pose2d withTranslation(Pose2d pose, Translation2d translation) {
+ return new Pose2d(translation, pose.getRotation());
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different rotation value.
+ *
+ * @param pose The original pose
+ * @param rotation The new rotation to use
+ * @return The new pose with the original translation and new rotation
+ */
+ public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) {
+ return new Pose2d(pose.getTranslation(), rotation);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import java.util.List;
+
+import org.wpilib.math.util.MathUtil;
+import frc.robot.constants.Constants;
+
+/** Utility class for useful functions. */
+public class MathUtils {
+
+ /**
+ * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
+ * (deadband, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @param deadband The deadband
+ * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
+ */
+ public static double deadband(double input, double deadband) {
+ if (Math.abs(input) <= deadband) {
+ return 0;
+ } else if (Math.abs(input) == 1) {
+ return input;
+ } else {
+ return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
+ }
+ }
+
+ /**
+ * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs
+ * to be linear from (OIConstants.DEADBAND, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1]
+ */
+ public static double deadband(double input) {
+ return deadband(input, Constants.DEFAULT_DEADBAND);
+ }
+
+ /**
+ * An exponential function that maintains positive or negative sign.
+ *
+ * @param exponent the power to raise the base to
+ * @param base the base which will be raised to the power
+ * @return base to the power of exponent, maintaining sign of base
+ */
+ public static double expoMS(double base, double exponent) {
+ // weird stuff will happen if you don't put a number > 0 for controller inputs
+ double finVal = Math.pow(Math.abs(base), exponent);
+ if (base < 0) {
+ finVal *= -1;
+ }
+ return finVal;
+ }
+
+ /**
+ * Calculates Midpoint of two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return midpoint of 2 numbers on modulus number line
+ */
+ public static double modulusMidpoint(
+ double num1, double num2, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ return MathUtil.inputModulus(
+ (num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound);
+ }
+ return (num1 + num2) / 2;
+ }
+
+ /**
+ * Interpolates between two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param amount the amount to interpolate, 0 = first number, 1 = second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return interpolated value between 2 numbers on modulus number line
+ */
+ public static double modulusInterpolate(
+ double num1, double num2, double amount, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ if (num1 < num2) {
+ num1 += upperBound - lowerBound;
+ } else {
+ num2 += upperBound - lowerBound;
+ }
+ }
+ return MathUtil.inputModulus((1 - amount) * num1 + amount * num2, lowerBound, upperBound);
+ }
+
+ /**
+ * Calls {@link #mean(double...)}.
+ *
+ * @param data the list of data to find the mean of
+ * @return the mean of the data
+ */
+ public static double mean(List<Double> data) {
+ return mean(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the mean of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the mean of the data
+ */
+ public static double mean(double... data) {
+ double mean = 0;
+ for (double datum : data) {
+ mean += datum;
+ }
+ mean /= data.length;
+ return mean;
+ }
+
+ /**
+ * Calls {@link #stdDev(double...)}.
+ *
+ * @param data the list of data to find the standard deviation of
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(List<Double> data) {
+ return stdDev(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the standard deviation of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(double... data) {
+ if (data.length == 0 || data.length == 1) return 0;
+
+ double mean = mean(data);
+
+ double total = 0;
+ for (double datum : data) {
+ total += Math.pow(datum - mean, 2);
+ }
+ return Math.sqrt(total / (data.length - 1));
+ }
+
+ private static double[] doubleListToArray(List<Double> arrayList) {
+ return arrayList.stream().mapToDouble(Double::doubleValue).toArray();
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+public class ModifiedCRT {
+ private int gearOne;
+ private int gearTwo;
+ private int turretGear;
+
+ public ModifiedCRT(int gearOne, int gearTwo, int turretGear) {
+ this.gearOne = gearOne;
+ this.gearTwo = gearTwo;
+ this.turretGear = turretGear;
+ }
+
+ public double bruteForce(double encoderLeftRot, double encoderRightRot) {
+ double[] encoderLeft = new double[gearOne];
+ double[] encoderRight = new double[gearTwo];
+
+ // Adds all possible positons for encoder left
+ for (int n = 0; n < gearOne; n++) {
+ encoderLeft[n] = (n + encoderLeftRot) * (gearOne / turretGear);
+ }
+ // Gets all possible encoder two positions
+ for (int n = 0; n < gearTwo; n++) {
+ encoderRight[n] = (n + encoderRightRot) * (gearTwo / turretGear);
+ }
+
+ for (double a : encoderLeft) {
+ for (double b : encoderRight) {
+ if (a == b) {
+ return a;
+ }
+ }
+ }
+ return 0.0;
+ }
+
+ private long modInverse(long a, long m) {
+ long m0 = m, t, q;
+ long x0 = 0, x1 = 1;
+ if (m == 1) return 0;
+ while (a > 1) {
+ q = a / m;
+ t = m;
+ m = a % m;
+ a = t;
+
+ t = x0;
+ x0 = x1 - q * x0;
+ x1 = t;
+ }
+
+ if (x1 < 0) {
+ x1 = +m0;
+ }
+ return x1;
+ }
+
+ public double solve(double encoderLeftRot, double encoderRightRot) {
+ double r1 = encoderLeftRot * gearOne;
+ double r2 = encoderRightRot * gearTwo;
+
+ long m1 = gearOne;
+ long m2 = gearTwo;
+
+ long inv = modInverse(m1 % m2, m2);
+
+ double x = r1 + m1 * (((r2 - r1) * inv) % m2);
+ double combined = x % (m1 * m2);
+
+ return combined / turretGear;
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import frc.robot.constants.Constants;
+
+/** Utility class for easy creation of motor controllers. */
+public class MotorFactory {
+
+ private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60;
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // SPARK MAX
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Create a SparkMax with current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @param stallLimit the current limit to set at stall
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
+ SparkMax sparkMAX = new SparkMax(id, motortype);
+
+ sparkMAX.configure(
+ new SparkMaxConfig()
+ .voltageCompensation(Constants.ROBOT_VOLTAGE)
+ .smartCurrentLimit(stallLimit)
+ .idleMode(IdleMode.kBrake),
+ ResetMode.kResetSafeParameters,
+ PersistMode.kNoPersistParameters);
+ return sparkMAX;
+ }
+
+ /**
+ * Create a SparkMax with default current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAXDefault(int id, MotorType motortype) {
+ return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // TALON FX (Falcon 500 and Kraken X60)
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Creates a TalonFX with all current limit options. If you would like to use defaults it is
+ * recommended to use the other createTalonFX.. methods.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param StatorLimitEnable whether to enable stator limiting
+ * @param StatorCurrentLimit the current, in amps, to return to after the stator limit is
+ * triggered
+ * @param StatorTriggerThreshold the threshold current to trigger the stator limit
+ * @param StatorTriggerDuration the duration, in seconds, the current is above the threshold
+ * before triggering
+ * @param SupplyLimitEnable whether to enable supply limiting
+ * @param SupplyCurrentLimit the current, in amps, to return to after the supply limit is
+ * triggered
+ * @param SupplyTriggerThreshold the threshold current to trigger the supply limit
+ * @param SupplyTriggerDuration the duration, in seconds, the current is above the threshold
+ * before triggering
+ * @return A fully configured TalonFX
+ */
+ public static TalonFX createTalonFXFull(
+ int id,
+ CANBus CANBus,
+ boolean StatorLimitEnable,
+ double StatorCurrentLimit,
+ double StatorTriggerThreshold,
+ double StatorTriggerDuration,
+ boolean SupplyLimitEnable,
+ double SupplyCurrentLimit,
+ double SupplyTriggerThreshold,
+ double SupplyTriggerDuration) {
+
+ if (id == -1) {
+ return null;
+ }
+
+ TalonFX talon = new TalonFX(id, CANBus);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // See explanations for Supply and Stator limiting in FalconConstants.java
+ config.CurrentLimits =
+ new CurrentLimitsConfigs()
+ .withStatorCurrentLimitEnable(StatorLimitEnable)
+ .withStatorCurrentLimit(StatorCurrentLimit)
+ .withSupplyCurrentLimitEnable(SupplyLimitEnable)
+ .withSupplyCurrentLimit(SupplyCurrentLimit)
+ .withSupplyCurrentLowerLimit(SupplyTriggerThreshold)
+ .withSupplyCurrentLowerTime(SupplyTriggerDuration);
+
+ config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE);
+
+ talon.getConfigurator().apply(config);
+ talon.setNeutralMode(NeutralModeValue.Brake);
+
+ return talon;
+ }
+
+ /**
+ * Creates a TalonFX with all the default settings.
+ *
+ * @param id the id of the motor
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ */
+ public static TalonFX createTalonFX(int id, CANBus CANBus) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ Constants.TALONFX_STATOR_LIMIT_ENABLE,
+ Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD,
+ Constants.TALONFX_STATOR_TRIGGER_DURATION,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE,
+ Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD,
+ Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
+
+ /**
+ * Creates a TalonFX with supply current limit options.
+ *
+ * <p>Supply current is current that's being drawn at the input bus voltage. Supply limiting is
+ * useful for preventing breakers from tripping in the PDP.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the supply limit is triggered
+ * @param triggerThreshold the threshold current to trigger the supply limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before
+ * triggering
+ */
+ public static TalonFX createTalonFXSupplyLimit(
+ int id, CANBus CANBus, double currentLimit, double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ Constants.TALONFX_STATOR_LIMIT_ENABLE,
+ Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD,
+ Constants.TALONFX_STATOR_TRIGGER_DURATION,
+ true,
+ currentLimit,
+ triggerThreshold,
+ triggerDuration);
+ }
+
+ /**
+ * Creates a TalonFX with stator current limit options.
+ *
+ * <p>Stator current is current that’s being drawn by the motor. Stator limiting is useful for
+ * limiting acceleration/heat.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the stator limit is triggered
+ * @param triggerThreshold the threshold current to trigger the stator limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before
+ * triggering
+ */
+ public static TalonFX createTalonFXStatorLimit(
+ int id, CANBus CANBus, double currentLimit, double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ true,
+ currentLimit,
+ triggerThreshold,
+ triggerDuration,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE,
+ Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD,
+ Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.system.Filesystem;
+import frc.robot.constants.AutoConstants;
+
+import java.io.File;
+import java.util.HashMap;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+/** Utility class for loading paths using pathplanner. */
+public class PathGroupLoader {
+
+ // private static final HashMap<String, List<PathPlannerTrajectory>> pathGroups = new HashMap<>();
+ private static final HashMap<String, PathPlannerPath> pathGroups = new HashMap<>();
+
+ /**
+ * Loads all the paths in the trajectory directory (specified in the constants). These paths are
+ * loaded and stored so that they do not take time while the robot is running and can be accessed
+ * with {@link #getPathGroup(String)}
+ */
+ public static void loadPathGroups() {
+ double totalTime = 0;
+ File[] directoryListing =
+ Filesystem.getDeployDirectory()
+ .toPath()
+ .resolve(AutoConstants.TRAJECTORY_DIRECTORY)
+ .toFile()
+ .listFiles();
+
+ if (directoryListing != null) {
+ for (File file : directoryListing) {
+ if (file.isFile() && file.getName().contains(".")) {
+ try {
+ long startTime = System.nanoTime();
+ String name = file.getName().substring(0, file.getName().lastIndexOf("."));
+ // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name));
+ pathGroups.put(name, PathPlannerPath.fromPathFile(name));
+ double time = (System.nanoTime() - startTime) / 1000000.0;
+ totalTime += time;
+ System.out.println(
+ "Processed file: " + file.getName() + ", took " + time + " milliseconds.");
+ } catch (Exception e) {
+ DriverStation.reportError(e.getMessage(), true);
+ }
+ }
+ }
+ } else {
+ System.out.println("Error processing file");
+ DriverStation.reportWarning("Issue with finding path files. Paths will not be loaded.", true);
+ }
+ System.out.println("File processing took a total of " + totalTime + " milliseconds");
+ }
+
+ /**
+ * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a
+ * list of trajectories that path planner can run.
+ *
+ * @param pathGroupName the name of the file, without any extensions. This should be the same
+ * exact name that is displayed in pathplanner
+ * @return a list of trajectories that path planner can run.
+ */
+ public static PathPlannerPath getPathGroup(String pathGroupName) {
+ if (pathGroups.get(pathGroupName) == null) {
+ System.out.println("Error retrieving " + pathGroupName + " path!");
+ }
+ return pathGroups.get(pathGroupName);
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.system.RobotController;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Queue;
+import java.util.concurrent.ArrayBlockingQueue;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
+ *
+ * <p>This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using
+ * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling.
+ * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore
+ * time synchronization.
+ */
+public class PhoenixOdometryThread extends Thread {
+ private final Lock signalsLock =
+ new ReentrantLock(); // Prevents conflicts when registering signals
+ private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
+ private final List<DoubleSupplier> genericSignals = new ArrayList<>();
+ private final List<Queue<Double>> phoenixQueues = new ArrayList<>();
+ private final List<Queue<Double>> genericQueues = new ArrayList<>();
+ private final List<Queue<Double>> timestampQueues = new ArrayList<>();
+
+ private static PhoenixOdometryThread instance = null;
+
+ public static PhoenixOdometryThread getInstance() {
+ if (instance == null) {
+ instance = new PhoenixOdometryThread();
+ }
+ return instance;
+ }
+
+ private PhoenixOdometryThread() {
+ setName("PhoenixOdometryThread");
+ setDaemon(true);
+ }
+
+ @Override
+ public void start() {
+ if (timestampQueues.size() > 0) {
+ super.start();
+ }
+ }
+
+ /** Registers a Phoenix signal to be read from the thread. */
+ public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ Drivetrain.odometryLock.lock();
+ try {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1];
+ System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
+ newSignals[phoenixSignals.length] = signal;
+ phoenixSignals = newSignals;
+ phoenixQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Registers a generic signal to be read from the thread. */
+ public Queue<Double> registerSignal(DoubleSupplier signal) {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ Drivetrain.odometryLock.lock();
+ try {
+ genericSignals.add(signal);
+ genericQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Returns a new queue that returns timestamp values for each sample. */
+ public Queue<Double> makeTimestampQueue() {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ Drivetrain.odometryLock.lock();
+ try {
+ timestampQueues.add(queue);
+ } finally {
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ @Override
+ public void run() {
+ while (true) {
+ // Wait for updates from all signals
+ signalsLock.lock();
+ try {
+ BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);
+ } finally {
+ signalsLock.unlock();
+ }
+
+ // Save new data to queues
+ Drivetrain.odometryLock.lock();
+ try {
+ // Sample timestamp is current FPGA time minus average CAN latency
+ // Default timestamps from Phoenix are NOT compatible with
+ // FPGA timestamps, this solution is imperfect but close
+ double timestamp = RobotController.getFPGATime() / 1e6;
+ double totalLatency = 0.0;
+ for (BaseStatusSignal signal : phoenixSignals) {
+ totalLatency += signal.getTimestamp().getLatency();
+ }
+ if (phoenixSignals.length > 0) {
+ timestamp -= totalLatency / phoenixSignals.length;
+ }
+
+ // Add new samples to queues
+ for (int i = 0; i < phoenixSignals.length; i++) {
+ phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble());
+ }
+ for (int i = 0; i < genericSignals.size(); i++) {
+ genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
+ }
+ for (int i = 0; i < timestampQueues.size(); i++) {
+ timestampQueues.get(i).offer(timestamp);
+ }
+ } finally {
+ Drivetrain.odometryLock.unlock();
+ }
+ }
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import java.util.function.Supplier;
+
+public class PhoenixUtil {
+ /** Attempts to run the command until no error is produced. */
+ public static void tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
+ for (int i = 0; i < maxAttempts; i++) {
+ var error = command.get();
+ if (error.isOK()) break;
+ }
+ }
+
+ /** Signals for synchronized refresh. */
+ private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0];
+
+ private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0];
+
+ /** Registers a set of signals for synchronized refresh. */
+ public static void registerSignals(boolean canivore, BaseStatusSignal... signals) {
+ if (canivore) {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[canivoreSignals.length + signals.length];
+ System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length);
+ System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length);
+ canivoreSignals = newSignals;
+ } else {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[rioSignals.length + signals.length];
+ System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length);
+ System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length);
+ rioSignals = newSignals;
+ }
+ }
+
+ /** Refresh all registered signals. */
+ public static void refreshAll() {
+ if (canivoreSignals.length > 0) {
+ BaseStatusSignal.refreshAll(canivoreSignals);
+ }
+ if (rioSignals.length > 0) {
+ BaseStatusSignal.refreshAll(rioSignals);
+ }
+ }
+}
--- /dev/null
+// 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.util;
+
+import java.util.Arrays;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Twist2d;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Stores and updates the position of each module */
+public class SwerveModulePose {
+
+ private double[] dist = {0, 0, 0, 0};
+ private Translation2d[] moduleTranslations;
+ private Pose2d[] modulePositions;
+ private double[] angles;
+ private Drivetrain drive;
+ private double prevRotation;
+ private Pose2d[] displayPoses;
+
+ /**
+ * Creates a new SwerveModulePose object to store and update the positions of each module
+ *
+ * @param drive The drivetrain
+ * @param modulePositions The translations of the modules relative to the center of the robot
+ */
+ public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions) {
+ this.drive = drive;
+ this.moduleTranslations = modulePositions;
+ this.modulePositions = new Pose2d[4];
+ angles = new double[4];
+ reset();
+ update();
+ reset();
+ }
+
+ /** Updates the module positions */
+ public void update() {
+ SwerveModuleState[] states = drive.getModuleStates();
+ double currentRotation = drive.getYaw().getRadians();
+ double chassisRotation = currentRotation - prevRotation;
+
+ for (int i = 0; i < 4; i++) {
+ double position = drive.getModules()[i].getPosition().distanceMeters;
+ double distance = position - dist[i];
+ dist[i] = position;
+
+ Twist2d twist =
+ new Twist2d(
+ distance,
+ 0,
+ MathUtil.angleModulus(states[i].angle.getRadians() - angles[i] + chassisRotation));
+ angles[i] = states[i].angle.getRadians();
+ modulePositions[i] = modulePositions[i].exp(twist);
+
+ displayPoses[i] =
+ new Pose2d(
+ modulePositions[i].getTranslation(),
+ EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01)
+ ? displayPoses[i].getRotation()
+ : states[i].speedMetersPerSecond < 0
+ ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI))
+ : modulePositions[i].getRotation());
+ }
+ prevRotation = currentRotation;
+ }
+
+ /**
+ * Gets the positions of the modules
+ *
+ * @return The module poses as an array of Pose2ds
+ */
+ public Pose2d[] getModulePoses() {
+ return displayPoses;
+ }
+
+ /** Resets the modules to the correct positions relative to the robot */
+ public void reset() {
+ Pose2d chassisPose2d = drive.getPose();
+ SwerveModuleState[] states = drive.getModuleStates();
+ for (int i = 0; i < 4; i++) {
+ angles[i] = states[i].angle.getRadians();
+ this.modulePositions[i] =
+ new Pose2d(
+ moduleTranslations[i]
+ .rotateBy(chassisPose2d.getRotation())
+ .plus(chassisPose2d.getTranslation()),
+ new Rotation2d(angles[i]).plus(chassisPose2d.getRotation()));
+ }
+ prevRotation = drive.getYaw().getRadians();
+ displayPoses = Arrays.copyOf(modulePositions, 4);
+ }
+
+ /**
+ * Gets whehter or not the modules have slipped A module has slipped if it has moved 0.3m (about
+ * 1ft) from its correct position relative to the other modules
+ *
+ * @return True if any of the modules have slipped, false otherwise
+ */
+ public boolean slipped() {
+ Translation2d total = new Translation2d();
+ for (Pose2d pose : modulePositions) {
+ total = total.plus(pose.getTranslation());
+ }
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ for (int i = 0; i < 4; i++) {
+ double dist =
+ modulePositions[i]
+ .relativeTo(drivePose)
+ .getTranslation()
+ .getDistance(moduleTranslations[i]);
+ if (dist > 0.3) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Gets the average slip distance
+ *
+ * @return The average distance between each module and its correct position
+ */
+ public double getAverageSlip() {
+ Translation2d total = new Translation2d();
+ for (Pose2d pose : modulePositions) {
+ total = total.plus(pose.getTranslation());
+ }
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ double slip = 0;
+ for (int i = 0; i < 4; i++) {
+ slip +=
+ modulePositions[i]
+ .relativeTo(drivePose)
+ .getTranslation()
+ .getDistance(moduleTranslations[i]);
+ }
+ return slip / 4;
+ }
+}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+public record ModuleLimits(
+ double maxDriveVelocity,
+ double maxDriveAcceleration,
+ double staticFriction,
+ double maxSteeringVelocity) {}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.SwerveModuleState;
+
+public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+import static frc.robot.util.EqualsUtil.*;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Twist2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModuleState;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.GeomUtil;
+
+/**
+ * "Inspired" by FRC team 254. See the license file in the root directory of this project.
+ *
+ * <p>Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path
+ * follower), and outputs a new setpoint that respects all of the kinematic constraints on module
+ * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the
+ * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
+ * kinematically infeasible (and can result in wheel slip or robot heading drift as a result).
+ */
+public class SwerveSetpointGenerator {
+ private final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS;
+ private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS;
+
+ /**
+ * Check if it would be faster to go to the opposite of the goal heading (and reverse drive
+ * direction).
+ *
+ * @param prevToGoal The rotation from the previous state to the goal state (i.e.
+ * prev.inverse().rotateBy(goal)).
+ * @return True if the shortest path to achieve this rotation involves flipping the drive
+ * direction.
+ */
+ private boolean flipHeading(Rotation2d prevToGoal) {
+ return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0;
+ }
+
+ private double unwrapAngle(double ref, double angle) {
+ double diff = angle - ref;
+ if (diff > Math.PI) {
+ return angle - 2.0 * Math.PI;
+ } else if (diff < -Math.PI) {
+ return angle + 2.0 * Math.PI;
+ } else {
+ return angle;
+ }
+ }
+
+ @FunctionalInterface
+ private interface Function2d {
+ double f(double x, double y);
+ }
+
+ /**
+ * Find the root of the generic 2D parametric function 'func' using the regula falsi technique.
+ * This is a pretty naive way to do root finding, but it's usually faster than simple bisection
+ * while being robust in ways that e.g. the Newton-Raphson method isn't.
+ *
+ * @param func The Function2d to take the root of.
+ * @param x_0 x value of the lower bracket.
+ * @param y_0 y value of the lower bracket.
+ * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during
+ * recursion)
+ * @param x_1 x value of the upper bracket.
+ * @param y_1 y value of the upper bracket.
+ * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during
+ * recursion)
+ * @param iterations_left Number of iterations of root finding left.
+ * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the
+ * (approximate) root.
+ */
+ private double findRoot(
+ Function2d func,
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ int iterations_left) {
+ if (iterations_left < 0 || epsilonEquals(f_0, f_1)) {
+ return 1.0;
+ }
+ var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0)));
+ var x_guess = (x_1 - x_0) * s_guess + x_0;
+ var y_guess = (y_1 - y_0) * s_guess + y_0;
+ var f_guess = func.f(x_guess, y_guess);
+ if (Math.signum(f_0) == Math.signum(f_guess)) {
+ // 0 and guess on same side of root, so use upper bracket.
+ return s_guess
+ + (1.0 - s_guess)
+ * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1);
+ } else {
+ // Use lower bracket.
+ return s_guess
+ * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1);
+ }
+ }
+
+ protected double findSteeringMaxS(
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ double max_deviation,
+ int max_iterations) {
+ f_1 = unwrapAngle(f_0, f_1);
+ double diff = f_1 - f_0;
+ if (Math.abs(diff) <= max_deviation) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+ double offset = f_0 + Math.signum(diff) * max_deviation;
+ Function2d func =
+ (x, y) -> {
+ return unwrapAngle(f_0, Math.atan2(y, x)) - offset;
+ };
+ return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+ }
+
+ protected double findDriveMaxS_254version(
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ double max_vel_step,
+ int max_iterations) {
+ double diff = f_1 - f_0;
+ if (Math.abs(diff) <= max_vel_step) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+ double offset = f_0 + Math.signum(diff) * max_vel_step;
+ Function2d func =
+ (x, y) -> {
+ return Math.hypot(x, y) - offset;
+ };
+ return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+ }
+
+ /**
+ * Limits the acceleration in all directions. This is different from findDriveMaxS because it
+ * includes the acceleration perpendicular to the wheel as it rotates. Given the same velocity
+ * step, this will return a lower S value than findDriveMaxS.
+ *
+ * @param x_0 The initial x velocity
+ * @param y_0 The initial y velocity
+ * @param x_1 The final x velocity
+ * @param y_1 The final y velocity
+ * @param max_vel_step The maxiumum amount the velocity can change this frame
+ * @param max_iterations The maximum number of iterations to use in findRoot
+ * @return The maximum interpolation value
+ */
+ protected double findAccelerationMaxS(
+ double x_0, double y_0, double x_1, double y_1, double max_vel_step, int max_iterations) {
+ double dist = Math.hypot(x_1 - x_0, y_1 - y_0);
+ if (dist <= max_vel_step) {
+ return 1;
+ }
+ return Math.max(0.0, Math.min(1.0, max_vel_step / dist));
+ }
+
+ protected double findDriveMaxS(
+ double x_0, double y_0, double x_1, double y_1, double max_vel_step) {
+ // Derivation:
+ // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the
+ // length of P(s) is the target T. P(s) is linearly interpolated between the
+ // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s).
+ // Then,
+ // T = sqrt(P(s).x^2 + P(s).y^2)
+ // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
+ // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
+ // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
+ // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
+ // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
+ // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
+ // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
+ // + (x_0^2 + y_0^2 - T^2).
+ //
+ // To simplify, we can factor out some common parts:
+ // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and
+ // p = x_0 * x_1 + y_0 * y_1.
+ // Then we have
+ // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
+ // with which we can solve for s using the quadratic formula.
+
+ double l_0 = x_0 * x_0 + y_0 * y_0;
+ double l_1 = x_1 * x_1 + y_1 * y_1;
+ double sqrt_l_0 = Math.sqrt(l_0);
+ double diff = Math.sqrt(l_1) - sqrt_l_0;
+ if (Math.abs(diff) <= max_vel_step) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+
+ double target = sqrt_l_0 + Math.copySign(max_vel_step, diff);
+ double p = x_0 * x_1 + y_0 * y_1;
+
+ // Quadratic of s
+ double a = l_0 + l_1 - 2 * p;
+ double b = 2 * (p - l_0);
+ double c = l_0 - target * target;
+ double root = Math.sqrt(b * b - 4 * a * c);
+
+ // Check if either of the solutions are valid
+ // Won't divide by zero because it is only possible for a to be zero if the
+ // target velocity is exactly the same or the reverse of the current
+ // velocity, which would be caught by the difference check.
+ double s_1 = (-b + root) / (2 * a);
+ if (isValidS(s_1)) {
+ return s_1;
+ }
+ double s_2 = (-b - root) / (2 * a);
+ if (isValidS(s_2)) {
+ return s_2;
+ }
+
+ // Since we passed the initial max_vel_step check, a solution should exist,
+ // but if no solution was found anyway, just don't limit movement
+ return 1.0;
+ }
+
+ protected static boolean isValidS(double s) {
+ return Double.isFinite(s) && s >= 0 && s <= 1;
+ }
+
+ /**
+ * Generate a new setpoint.
+ *
+ * @param limits The kinematic limits to respect for this setpoint.
+ * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground.
+ * This assumes that the center of mass is in the center of the robot in the x and y
+ * directions. If tipping is not a potential problem this year, set this to 0.
+ * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
+ * iteration setpoint instead of the actual measured/estimated kinematic state.
+ * @param desiredState The desired state of motion, such as from the driver sticks or a path
+ * following algorithm.
+ * @param dt The loop time.
+ * @return A Setpoint object that satisfies all of the KinematicLimits while converging to
+ * desiredState quickly.
+ */
+ public SwerveSetpoint generateSetpoint(
+ final ModuleLimits limits,
+ double centerOfMassHeight,
+ final SwerveSetpoint prevSetpoint,
+ ChassisSpeeds desiredState,
+ double dt) {
+ final Translation2d[] modules = moduleLocations;
+
+ SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState);
+ // Make sure desiredState respects velocity limits.
+ if (limits.maxDriveVelocity() > 0.0) {
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
+ desiredState = kinematics.toChassisSpeeds(desiredModuleState);
+ }
+
+ // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
+ // just use the previous angle.
+ boolean need_to_steer = true;
+ if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) {
+ need_to_steer = false;
+ for (int i = 0; i < modules.length; ++i) {
+ desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle;
+ desiredModuleState[i].speedMetersPerSecond = 0.0;
+ }
+ }
+
+ // For each module, compute local Vx and Vy vectors.
+ double[] prev_vx = new double[modules.length];
+ double[] prev_vy = new double[modules.length];
+ Rotation2d[] prev_heading = new Rotation2d[modules.length];
+ double[] desired_vx = new double[modules.length];
+ double[] desired_vy = new double[modules.length];
+ Rotation2d[] desired_heading = new Rotation2d[modules.length];
+ boolean all_modules_should_flip = true;
+ for (int i = 0; i < modules.length; ++i) {
+ prev_vx[i] =
+ prevSetpoint.moduleStates()[i].angle.getCos()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_vy[i] =
+ prevSetpoint.moduleStates()[i].angle.getSin()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_heading[i] = prevSetpoint.moduleStates()[i].angle;
+ if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) {
+ prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ desired_vx[i] =
+ desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond;
+ desired_vy[i] =
+ desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond;
+ desired_heading[i] = desiredModuleState[i].angle;
+ if (desiredModuleState[i].speedMetersPerSecond < 0.0) {
+ desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ if (all_modules_should_flip) {
+ double required_rotation_rad =
+ Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians());
+ if (required_rotation_rad < Math.PI / 2.0) {
+ all_modules_should_flip = false;
+ }
+ }
+ }
+ if (all_modules_should_flip
+ && !EqualsUtil.GeomExtensions.epsilonEquals(
+ GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()), new Twist2d())
+ && !EqualsUtil.GeomExtensions.epsilonEquals(
+ GeomUtil.toTwist2d(desiredState), new Twist2d())) {
+ // It will (likely) be faster to stop the robot, rotate the modules in place to the complement
+ // of the desired
+ // angle, and accelerate again.
+ return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt);
+ }
+
+ // Compute the deltas between start and goal. We can then interpolate from the start state to
+ // the goal state; then
+ // find the amount we can move from start towards goal in this cycle such that no kinematic
+ // limit is exceeded.
+ double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
+ double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
+ double dtheta =
+ desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+
+ // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at
+ // desiredState.
+ double min_s = 1.0;
+
+ // In cases where an individual module is stopped, we want to remember the right steering angle
+ // to command (since
+ // inverse kinematics doesn't care about angle, we can be opportunistically lazy).
+ List<Optional<Rotation2d>> overrideSteering = new ArrayList<>(modules.length);
+ // Enforce steering velocity limits. We do this by taking the derivative of steering angle at
+ // the current angle,
+ // and then backing out the maximum interpolant between start and goal states. We remember the
+ // minimum across all modules, since
+ // that is the active constraint.
+ final double max_theta_step = dt * limits.maxSteeringVelocity();
+ for (int i = 0; i < modules.length; ++i) {
+ if (!need_to_steer) {
+ overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle));
+ continue;
+ }
+ overrideSteering.add(Optional.empty());
+ if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) {
+ // If module is stopped, we know that we will need to move straight to the final steering
+ // angle, so limit based
+ // purely on rotation in place.
+ if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) {
+ // Goal angle doesn't matter. Just leave module at its current angle.
+ overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle));
+ continue;
+ }
+
+ var necessaryRotation =
+ prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle);
+ if (flipHeading(necessaryRotation)) {
+ necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ // getRadians() bounds to +/- Pi.
+ final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step;
+
+ if (numStepsNeeded <= 1.0) {
+ // Steer directly to goal angle.
+ overrideSteering.set(i, Optional.of(desiredModuleState[i].angle));
+ // Don't limit the global min_s;
+ continue;
+ } else {
+ // Adjust steering by max_theta_step.
+ overrideSteering.set(
+ i,
+ Optional.of(
+ prevSetpoint.moduleStates()[i].angle.rotateBy(
+ Rotation2d.fromRadians(
+ Math.signum(necessaryRotation.getRadians()) * max_theta_step))));
+ min_s = 0.0;
+ continue;
+ }
+ }
+ if (min_s == 0.0) {
+ // s can't get any lower. Save some CPU.
+ continue;
+ }
+
+ final int kMaxIterations = 8;
+ double s =
+ findSteeringMaxS(
+ prev_vx[i],
+ prev_vy[i],
+ prev_heading[i].getRadians(),
+ desired_vx[i],
+ desired_vy[i],
+ desired_heading[i].getRadians(),
+ max_theta_step,
+ kMaxIterations);
+ min_s = Math.min(min_s, s);
+ }
+
+ // Enforce drive wheel acceleration limits.
+ final double max_vel_step = dt * limits.maxDriveAcceleration();
+ final double max_vel_step_2 = dt * limits.staticFriction() * Constants.GRAVITY_ACCELERATION;
+ for (int i = 0; i < modules.length; ++i) {
+ if (min_s == 0.0) {
+ // No need to carry on.
+ break;
+ }
+ double vx_min_s =
+ min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i];
+ double vy_min_s =
+ min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i];
+ // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we
+ // already know we can't go faster
+ // than that.
+ final int kMaxIterations = 10;
+ double s = min_s * findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
+
+ // Limit the overall acceleration of this wheel
+ double s2 =
+ min_s
+ * findAccelerationMaxS(
+ prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
+
+ min_s = Math.min(Math.min(min_s, s), s2);
+ }
+
+ if (centerOfMassHeight > 0.02) {
+ // Limit the acceleration in the x and y directions separately based on the center of mass.
+ // To make the torque on the robot 0, we can assume all of the mass is on the back wheel,
+ // where the front is the direction the robot is accelerating toward
+ // Torque is equal to the force times the component of the radius perpendicular to the force
+ // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center
+ // to wheel
+ // T = mgx - mah = 0
+ // a = gx/h
+ double maxAccel =
+ Constants.GRAVITY_ACCELERATION * (DriveConstants.TRACK_WIDTH / 2) / centerOfMassHeight;
+ // Limit based on this calculated value
+ // x and y are limited separately because, when tipping in a diagonal direction, the distance
+ // is longer
+ double xAccel =
+ Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond)
+ / dt;
+ double yAccel =
+ Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond)
+ / dt;
+ if (!epsilonEquals(xAccel, 0)) {
+ double s = maxAccel / xAccel;
+ min_s = Math.min(min_s, s);
+ }
+ if (!epsilonEquals(yAccel, 0)) {
+ double s = maxAccel / yAccel;
+ min_s = Math.min(min_s, s);
+ }
+ }
+
+ ChassisSpeeds retSpeeds =
+ new ChassisSpeeds(
+ prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
+ prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
+ prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
+ var retStates = kinematics.toSwerveModuleStates(retSpeeds);
+ for (int i = 0; i < modules.length; ++i) {
+ final var maybeOverride = overrideSteering.get(i);
+ if (maybeOverride.isPresent()) {
+ var override = maybeOverride.get();
+ if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) {
+ retStates[i].speedMetersPerSecond *= -1.0;
+ }
+ retStates[i].angle = override;
+ }
+ final var deltaRotation =
+ prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
+ if (flipHeading(deltaRotation)) {
+ retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI));
+ retStates[i].speedMetersPerSecond *= -1.0;
+ }
+ }
+ return new SwerveSetpoint(retSpeeds, retStates);
+ }
+}
--- /dev/null
+// 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.util;
+
+import org.wpilib.units.measure.Voltage;
+import org.wpilib.sysid.SysIdRoutineLog;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Subsystem;
+import org.wpilib.command2.sysid.SysIdRoutine;
+import org.wpilib.command2.sysid.SysIdRoutine.Config;
+import org.wpilib.command2.sysid.SysIdRoutine.Direction;
+import org.wpilib.command2.sysid.SysIdRoutine.Mechanism;
+
+import java.util.function.Consumer;
+
+/** Util class for creating SysId routines */
+public class SysId {
+
+ private SysIdRoutine sysIdRoutine;
+
+ public SysId(
+ String name,
+ Consumer<Voltage> driveConsumer,
+ Consumer<SysIdRoutineLog> logConsumer,
+ Subsystem subsystem,
+ Config config) {
+ sysIdRoutine =
+ new SysIdRoutine(config, new Mechanism(driveConsumer, logConsumer, subsystem, name));
+ }
+
+ public SysId(String name, Consumer<Voltage> driveConsumer, Subsystem subsystem, Config config) {
+ this(name, driveConsumer, null, subsystem, config);
+ }
+
+ public Command runQuasisStatic(Direction direction) {
+ return sysIdRoutine.quasistatic(direction);
+ }
+
+ public Command runDynamic(Direction direction) {
+ return sysIdRoutine.dynamic(direction);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.wpilib.util.WPIUtilJNI;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Class for testing if a value is within a certain margin of error for a certain amount of time.
+ */
+public class TimeAccuracyTest {
+
+ private final BooleanSupplier accuracyTest;
+ private final double setpointUpdateTime;
+ private final double errorMargin;
+ private final double timeMargin;
+ private boolean lastUseableResult = false;
+
+ /**
+ * @param actual DoubleSupplier that returns the actual value
+ * @param setpoint DoubleSupplier that returns the setpoint
+ * @param errorMargin margin of error for the test to be accurate
+ * @param timeMargin time in seconds that the setpoint must be held for the test to be accurate
+ */
+ public TimeAccuracyTest(
+ DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) {
+ this.errorMargin = errorMargin;
+ this.timeMargin = timeMargin;
+ setpointUpdateTime = WPIUtilJNI.now() * 1e-6;
+ accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint);
+ }
+
+ /**
+ * Determines if the test is successful.
+ *
+ * @return true if the test is successful, false if not
+ */
+ public boolean calculate() {
+ if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6)
+ lastUseableResult = accuracyTest.getAsBoolean();
+ return lastUseableResult;
+ }
+
+ /**
+ * Determines if the actual value is within the error margin of the setpoint.
+ *
+ * @return true if the actual value is within the error margin of the setpoint, false if not
+ */
+ private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) {
+ return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin;
+ }
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.util.Units;
+import org.wpilib.driverstation.DriverStation.Alliance;
+import frc.robot.Robot;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** Stores information about an object detected by vision */
+public class DetectedObject {
+ private static Drivetrain drive;
+ public final Pose3d pose;
+ public final ObjectType type;
+
+ public enum ObjectType {
+ CORAL(Units.inchesToMeters(4.5 / 2)),
+ ALGAE(Units.inchesToMeters(16.25 / 2)),
+ RED_ROBOT(0),
+ BLUE_ROBOT(0),
+ NONE(0);
+
+ public final double height;
+
+ private ObjectType(double h) {
+ height = h;
+ }
+ };
+
+ /**
+ * Sets the drivetrain to use for pose calculations
+ *
+ * @param drive The drivetrain
+ */
+ public static void setDrive(Drivetrain drive) {
+ DetectedObject.drive = drive;
+ }
+
+ /** Creates a default DetectedObject with default attributes */
+ public DetectedObject() {
+ pose = new Pose3d();
+ type = ObjectType.NONE;
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, type, robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ ObjectType type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation =
+ new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate and translate it to get the position relative to the robot
+ .rotateBy(robotToCamera.getRotation())
+ .plus(robotToCamera.getTranslation());
+ // If the drivetrain exists, rotate and translate it to get the field relative position
+ if (drive != null) {
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation =
+ translation
+ .rotateBy(new Rotation3d(0, 0, drivePose.getRotation().getRadians()))
+ .plus(new Translation3d(drivePose.getX(), drivePose.getY(), 0));
+ }
+ pose = new Pose3d(translation, new Rotation3d());
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ int type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ String type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, type, robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ ObjectType type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation =
+ new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate it to get the position relative to the rotated camera
+ .rotateBy(robotToCamera.getRotation());
+ // Scale it so that the object will be on the ground (- because translation's z will be
+ // negative)
+ if (!isRobot()) {
+ translation = translation.times(-(robotToCamera.getZ() - type.height) / translation.getZ());
+ } else {
+ // Assume all robots are ~3m from the camera
+ translation = translation.times(3);
+ }
+ // Translate it to make it relative to the robot
+ translation = translation.plus(robotToCamera.getTranslation());
+ // If the drivetrain exists, rotate and translate it to be field relative
+ if (drive != null) {
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation =
+ translation
+ .rotateBy(new Rotation3d(0, 0, drivePose.getRotation().getRadians()))
+ .plus(new Translation3d(drivePose.getX(), drivePose.getY(), 0));
+ }
+ pose = new Pose3d(translation, new Rotation3d());
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp) {
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp) {
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Converts an int to an ObjectType
+ *
+ * @param type The type as an int, between 0 and the number of object types - 1
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(int type) {
+ ObjectType[] values = ObjectType.values();
+ if (type < 0 || type >= values.length) {
+ return ObjectType.NONE;
+ }
+ return values[type];
+ }
+
+ /**
+ * Converts a String to an ObjectType
+ *
+ * @param type The type as a String
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(String type) {
+ ObjectType result = ObjectType.valueOf(type.toUpperCase());
+ return result == null ? ObjectType.NONE : result;
+ }
+
+ /**
+ * Returns if the object is a game piece
+ *
+ * @return True if the object is a game piece, false otherwise
+ */
+ public boolean isGamePiece() {
+ return type == ObjectType.CORAL || type == ObjectType.ALGAE;
+ }
+
+ /**
+ * Returns if the object is a robot
+ *
+ * @return True if the object is a red or blue robot, false otherwise
+ */
+ public boolean isRobot() {
+ return type == ObjectType.RED_ROBOT || type == ObjectType.BLUE_ROBOT;
+ }
+
+ /**
+ * Returns if the object is a robot on the same alliance
+ *
+ * @return If the object is a robot on the same alliance
+ */
+ public boolean isSameAllianceRobot() {
+ return type
+ == (Robot.getAlliance() == Alliance.Red ? ObjectType.RED_ROBOT : ObjectType.BLUE_ROBOT);
+ }
+
+ /**
+ * Returns if the object is a robot on the other alliance
+ *
+ * @return If the object is a robot on the other alliance
+ */
+ public boolean isOtherAllianceRobot() {
+ return type
+ == (Robot.getAlliance() == Alliance.Red ? ObjectType.BLUE_ROBOT : ObjectType.RED_ROBOT);
+ }
+
+ /**
+ * Gets the distance from the center of the robot to the object
+ *
+ * @return The distance in meters
+ */
+ public double getDistance() {
+ return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d());
+ }
+
+ /**
+ * Gets the field relative angle from the robot to the object
+ *
+ * @return The angle in radians
+ */
+ public double getAngle() {
+ Pose2d drivePose = drive.getPose();
+ return Math.atan2(pose.getY() - drivePose.getY(), pose.getX() - drivePose.getX());
+ }
+
+ /**
+ * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise)
+ *
+ * @return The relative angle in radians
+ */
+ public double getRelativeAngle() {
+ double angle = getAngle() - drive.getYaw().getRadians();
+ return MathUtil.angleModulus(angle);
+ }
+
+ /**
+ * Gets the angle of the object relative to the robot's velocity (0 is in front, positive
+ * counterclockwise)
+ *
+ * @return The relative angle in radians
+ */
+ public double getVelocityRelativeAngle() {
+ ChassisSpeeds speeds = drive.getChassisSpeeds();
+ double angle =
+ getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
+ return MathUtil.angleModulus(angle);
+ }
+
+ public String toString() {
+ return type + " at (" + pose.getX() + ", " + pose.getY() + ", " + pose.getZ() + ")";
+ }
+}
--- /dev/null
+// 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.util.Vision;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.trajectory.TrapezoidProfile;
+import org.wpilib.math.trajectory.TrapezoidProfile.Constraints;
+import org.wpilib.math.trajectory.TrapezoidProfile.State;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+
+/** A util class to assist the driver drive to a pose */
+public class DriverAssist {
+ // The amount to correct the driver's inpus by
+ // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed,
+ // sometimes equal to it
+ // This can be greater than 1 to fully correct more of the time, like from farther away
+ private static final double CORRECTION_FACTOR = 1;
+
+ // Variables used for first method
+ // The setpoint generator, which limits the acceleration
+ private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+ private static final TrapezoidProfile xProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile yProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile angleProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL));
+
+ /**
+ * Combines the driver input with a speed calculated using a trapezoidal profile
+ *
+ * <p>Called when VisionConstants.DRIVER_ASSIST_MODE is 2
+ *
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ private static ChassisSpeeds calculate2(
+ Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ // Do nothing if there is no pose
+ if (desiredPose == null) {
+ return driverInput;
+ }
+
+ // Store current states
+ Pose2d currentPose = drive.getPose();
+ Rotation2d yaw = drive.getYaw();
+ ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
+ driveSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ driveSpeeds,
+ yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new
+ // object
+ State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
+ State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
+ State angleState =
+ new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
+
+ // Store goal states
+ State xGoal = new State(desiredPose.getX(), 0);
+ State yGoal = new State(desiredPose.getY(), 0);
+ Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation());
+ double rotation =
+ keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians();
+ if (rotation - currentPose.getRotation().getRadians() > Math.PI) {
+ rotation -= 2 * Math.PI;
+ } else if (rotation - currentPose.getRotation().getRadians() < -Math.PI) {
+ rotation += 2 * Math.PI;
+ }
+ State angleGoal = new State(rotation, 0);
+
+ // Calculate ideal speeds for next frame
+ ChassisSpeeds goal =
+ new ChassisSpeeds(
+ xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity,
+ yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity,
+ angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity);
+ // Robot-relataive goal
+ ChassisSpeeds goalRobot = goal.times(1);
+ goalRobot = ChassisSpeeds.fromRobotRelativeSpeeds(goalRobot, yaw);
+
+ // This calculates the actual acceleration we can get
+ // This is the only thing that needs to be robot relative
+ SwerveSetpoint nextSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(),
+ goalRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
+ nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
+
+ // Robot relative driver inputs
+ ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
+ driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
+ // This is the speed the driver will be able to get next frame
+ // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
+ SwerveSetpoint driverSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(),
+ driverInputRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
+ driverSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds, yaw);
+
+ // The difference between the 2 speeds
+ ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
+
+ // 1.2*1.2^-distance decreases the amount it correct by as distance increases
+ double distanceFactor =
+ 1.2
+ * Math.pow(
+ 1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation()));
+
+ // Driver input speed
+ double driverInputSpeed =
+ Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+
+ // The amount to correct by
+ ChassisSpeeds correction =
+ error.times(
+ Math.min(
+ CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED,
+ 1));
+
+ return driverSpeeds.plus(correction);
+ // return
+ // nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
+ }
+
+ // Constants used for second method
+ public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI / 4;
+ public static final double MAX_DISTANCE_ERROR = 2;
+ public static final double ROTATION_CORRECTION_FACTOR = 0.1;
+ public static final double MAX_ROTATION_ERROR = Math.PI / 3;
+
+ /**
+ * Combines the driver input with a calculated correction speed
+ *
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ @SuppressWarnings(
+ "unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
+ public static ChassisSpeeds calculate(
+ Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ if (VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null) {
+ return driverInput;
+ } else if (VisionConstants.DRIVER_ASSIST_MODE == 2) {
+ return calculate2(drive, driverInput, desiredPose, keepAngle);
+ }
+ // Combines the driver input with a speed perpendicular to the input
+ Pose2d drivePose = drive.getPose();
+ Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation());
+ double distance = difference.getNorm();
+ double velocityAngle = difference.getAngle().getRadians();
+ double targetAngle =
+ keepAngle
+ ? desiredPose.getRotation().getRadians()
+ : MathUtil.angleModulus(velocityAngle + Math.PI / 2);
+ double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+ double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
+ double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
+ if (Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR) {
+ return driverInput;
+ }
+ double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError));
+ if (perpendicularDist > MAX_DISTANCE_ERROR) {
+ return driverInput;
+ }
+ double perpendicularAngle =
+ MathUtil.angleModulus(driverAngle + Math.PI / 2 * Math.signum(velocityAngleError));
+ // Different options for calculation.
+ double correctionSpeed = 0;
+ switch (VisionConstants.DRIVER_ASSIST_MODE) {
+ case 3:
+ correctionSpeed =
+ Math.min(
+ CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist),
+ Math.abs(Math.tan(velocityAngleError) * inputSpeed));
+ break;
+ case 4:
+ correctionSpeed =
+ Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1)
+ * Math.abs(Math.tan(velocityAngleError) * inputSpeed);
+ break;
+ case 5:
+ correctionSpeed =
+ Math.min(
+ CORRECTION_FACTOR
+ * inputSpeed
+ * Math.pow(2, -perpendicularDist)
+ * Math.pow(1.2, -distance + 1),
+ Math.abs(Math.tan(velocityAngleError) * inputSpeed));
+ break;
+ }
+ double rotationError =
+ MathUtil.angleModulus(targetAngle - drivePose.getRotation().getRadians());
+ if (Math.abs(rotationError) > MAX_ROTATION_ERROR) {
+ return driverInput;
+ }
+ // We want to set the current angular velocity so that we can decelerate to 0rad/s at the
+ // setpoint
+ // Since 0=v0^2+2ax, v0=√(2ax)
+ // High correction factors will also ignore the driver's input more
+ double rotationalSpeed =
+ ROTATION_CORRECTION_FACTOR
+ * Math.signum(rotationError)
+ * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError))
+ - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
+ return driverInput.plus(
+ new ChassisSpeeds(
+ correctionSpeed * Math.cos(perpendicularAngle),
+ correctionSpeed * Math.sin(perpendicularAngle),
+ rotationalSpeed));
+ }
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.geometry.Pose3d;
+import frc.robot.constants.GyroBiasConstants;
+
+/**
+ * estimates gyro bias by comparing vision-derived yaw to gyro yaw.
+ *
+ * <p>when the robot observes April tags, PhotonVision calculates what the robot heading SHOULD be
+ * based on known tag positions vs observed angles. This can be compared to the gyro reading to
+ * detect and correct drift.
+ */
+public class GyroBiasEstimator {
+
+ private double weightedBiasSum = 0.0;
+ private double totalWeight = 0.0;
+ private int sampleCount = 0;
+
+ // exponential moving average
+ private double emaBias = 0.0;
+ private boolean emaInitialized = false;
+
+ /**
+ * process a new observation comparing vision pose to gyro reading.
+ *
+ * @param visionPose the pose estimated by vision (from PhotonVision)
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied (has enough samples)
+ */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw, double visionWeight) {
+ if (visionPose == null) {
+ return false;
+ }
+
+ // get yaw from vision
+ double visionYaw = visionPose.getRotation().getZ();
+
+ return addObservation(visionYaw, gyroYaw, visionWeight);
+ }
+
+ /**
+ * process a new observation with just yaw values.
+ *
+ * @param visionYaw yaw from vision pose in radians
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for this observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied
+ */
+ public boolean addObservation(double visionYaw, double gyroYaw, double visionWeight) {
+ // normalize to [-PI, PI]
+ double diff = normalizeAngle(visionYaw - gyroYaw);
+
+ // reject outliers
+ if (Math.abs(diff) > GyroBiasConstants.MAX_ANGLE_DIFF_RAD) {
+ return false;
+ }
+
+ // clamp weight
+ double weight = Math.max(0.0, Math.min(1.0, visionWeight));
+
+ // accumulate weighted bias
+ weightedBiasSum += diff * weight;
+ totalWeight += weight;
+ sampleCount++;
+
+ // update exponential moving average
+ if (!emaInitialized) {
+ emaBias = diff;
+ emaInitialized = true;
+ } else {
+ emaBias = emaBias * (1.0 - GyroBiasConstants.EMA_ALPHA) + diff * GyroBiasConstants.EMA_ALPHA;
+ }
+
+ return sampleCount >= GyroBiasConstants.MIN_SAMPLES;
+ }
+
+ /** process new observation with default weight of 1.0. maintains backward compatibility. */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw) {
+ return addObservation(visionPose, gyroYaw, 1.0);
+ }
+
+ /** process new observation with default weight of 1.0. maintains backward compatibility */
+ public boolean addObservation(double visionYaw, double gyroYaw) {
+ return addObservation(visionYaw, gyroYaw, 1.0);
+ }
+
+ /**
+ * get average bias and reset
+ *
+ * @return average bias in radians to apply, or 0 if not enough samples
+ */
+ public double getAndResetBias() {
+ if (sampleCount < GyroBiasConstants.MIN_SAMPLES) {
+ return 0.0;
+ }
+
+ // use weighted average
+ double avgBias = weightedBiasSum / totalWeight;
+
+ // reset
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+
+ return avgBias;
+ }
+
+ /**
+ * apply partial correction to avoid sudden jumps.
+ *
+ * @param fullBias the full calculated bias
+ * @return partial correction to apply
+ */
+ public double applyPartialCorrection(double fullBias) {
+ double clampedBias = fullBias;
+ if (clampedBias > GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
+ } else if (clampedBias < -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
+ }
+
+ return clampedBias * GyroBiasConstants.CORRECTION_FRACTION;
+ }
+
+ /** normalize angle to [-PI, PI] */
+ private double normalizeAngle(double angle) {
+ return MathUtil.angleModulus(angle);
+ }
+
+ /** get sample count for debugging */
+ public int getSampleCount() {
+ return sampleCount;
+ }
+
+ /** get current accumulated bias without resetting */
+ public double getCurrentBias() {
+ if (sampleCount == 0) {
+ return 0.0;
+ }
+ if (totalWeight > 0) {
+ return weightedBiasSum / totalWeight;
+ }
+ return emaBias;
+ }
+
+ /** get current total weight for debugging */
+ public double getTotalWeight() {
+ return totalWeight;
+ }
+
+ /** reset accumulated state */
+ public void reset() {
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+ }
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.DoubleUnaryOperator;
+
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout.OriginPosition;
+import org.wpilib.math.util.Pair;
+import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableEntry;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.framework.RobotBase;
+import org.wpilib.system.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.MathUtils;
+
+// Vision and it's commands are adapted from Iron Claw's FRC2023
+public class Vision {
+ private NetworkTable objectDetectionTable;
+
+ private NetworkTableEntry xOffset;
+ private NetworkTableEntry yOffset;
+ private NetworkTableEntry objectDistance;
+ private NetworkTableEntry objectClass;
+ private NetworkTableEntry cameraIndex;
+
+ // A list of the cameras on the robot.
+ private ArrayList<VisionCamera> cameras = new ArrayList<>();
+
+ private VisionSystemSim visionSim;
+
+ private boolean sawTag = false;
+
+ // Array of tags to use, null or empty array to use all tags
+ private int[] onlyUse = null;
+
+ /** Creates a new instance of Vision and sets up the cameras and field layout */
+ public Vision(ArrayList<Pair<String, Transform3d>> camList) {
+ // Initialize object_detection NetworkTable
+ objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
+
+ // From the object detection NetworkTable, get the entries
+ objectDistance = objectDetectionTable.getEntry("distance");
+ xOffset = objectDetectionTable.getEntry("x_offset");
+ yOffset = objectDetectionTable.getEntry("y_offset");
+ objectClass = objectDetectionTable.getEntry("class");
+ cameraIndex = objectDetectionTable.getEntry("index");
+
+ // Start NetworkTables server
+ NetworkTableInstance.getDefault().startServer();
+
+ // Sets the origin to the right side of the blue alliance wall
+ FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+
+ if (VisionConstants.ENABLED) {
+ // Puts the cameras in an array list
+ for (int i = 0; i < camList.size(); i++) {
+ cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond()));
+ }
+
+ if (RobotBase.isSimulation()) {
+ visionSim = new VisionSystemSim("Vision");
+ visionSim.addAprilTags(FieldConstants.field);
+ for (VisionCamera c : cameras) {
+ PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
+ cameraSim.enableDrawWireframe(true);
+ cameraSim.prop.setAvgLatencyMs(30);
+ cameraSim.prop.setCalibration(1280, 720, Rotation2d.fromDegrees(78));
+ visionSim.addCamera(cameraSim, c.photonPoseEstimator.getRobotToCameraTransform());
+ }
+ }
+ }
+
+ Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()];
+ for (int i = 0; i < FieldConstants.field.getTags().size(); i++) {
+ tags[i] = (FieldConstants.field.getTagPose(i + 1).get());
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("AprilTags", tags);
+ }
+ }
+
+ /**
+ * Get the horizontal offsets from the crosshair to the targets
+ *
+ * @return An array of offsets in degrees
+ */
+ public double[] getHorizontalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
+ return new double[0];
+ }
+ return xOffset.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Get the vertical offsets from the crosshair to the targets
+ *
+ * @return An array of offsets in degrees
+ */
+ public double[] getVerticalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
+ return new double[0];
+ }
+ return yOffset.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Get the target distances
+ *
+ * @return Distance in meters
+ */
+ @SuppressWarnings("unused")
+ public double[] getDistance() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
+ return new double[0];
+ }
+ return objectDistance.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Returns whether or not a valid object is detected
+ *
+ * @return true or false
+ */
+ public boolean validObjectDetected() {
+ return getHorizontalOffset().length > 0;
+ }
+
+ /**
+ * Returns what types of object are detected
+ *
+ * @return The object types as a String array
+ */
+ @SuppressWarnings("unused")
+ public String[] getDetectedObjectClass() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
+ return new String[0];
+ }
+ return objectClass.getStringArray(new String[0]);
+ }
+
+ /**
+ * Gets the camera indices (which camera sees the object)
+ *
+ * @return The indices as a long array (method returns long array instead of int array)
+ */
+ @SuppressWarnings("unused")
+ public long[] getCameraIndex() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
+ return new long[0];
+ }
+ return cameraIndex.getIntegerArray(new long[0]);
+ }
+
+ /**
+ * Stores all of the detected objects in an array
+ *
+ * @return The array of DetectedObjects
+ */
+ public DetectedObject[] getDetectedObjects() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
+ return new DetectedObject[0];
+ }
+ double[] xOffset = getHorizontalOffset();
+ double[] yOffset = getVerticalOffset();
+ // double[] distance = getDistance();
+ String[] objectClass = getDetectedObjectClass();
+ // long[] cameraIndex = getCameraIndex();
+ DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)];
+ for (int i = 0; i < objects.length; i++) {
+ objects[i] =
+ new DetectedObject(
+ Units.degreesToRadians(xOffset[i]),
+ -Units.degreesToRadians(yOffset[i]),
+ // distance[i],
+ objectClass[i],
+ // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
+ }
+ return objects;
+ }
+
+ /**
+ * Returns the closest game piece in front of the robot
+ *
+ * @param maxAngle The maximum angle between the angle to the object and the robot's heading or
+ * rotation to use, in radians
+ * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true
+ * for heading
+ * @return The best DetectedObject
+ */
+ public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity) {
+ DetectedObject[] objects = getDetectedObjects();
+ DetectedObject best = null;
+ double closest = Double.POSITIVE_INFINITY;
+ for (DetectedObject object : objects) {
+ double dist = object.getDistance();
+ if (object.isGamePiece()
+ && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle())
+ < maxAngle
+ && dist < closest) {
+ closest = dist;
+ best = object;
+ }
+ }
+ return best;
+ }
+
+ /**
+ * Gets the pose as a Pose2d using PhotonVision
+ *
+ * @param referencePoses The reference poses in order of preference, null poses will be skipped
+ * @return The pose of the robot, or null if it can't see april tags
+ */
+ public Pose2d getPose2d(Pose2d... referencePoses) {
+ Pose2d referencePose = new Pose2d();
+ for (Pose2d checkReferencePose : referencePoses) {
+ if (checkReferencePose != null) {
+ referencePose = checkReferencePose;
+ break;
+ }
+ }
+ ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(referencePose);
+
+ if (estimatedPoses.size() == 0) return null;
+
+ if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d();
+
+ if (estimatedPoses.size() == 2) {
+ return new Pose2d(
+ estimatedPoses
+ .get(0)
+ .estimatedPose
+ .toPose2d()
+ .getTranslation()
+ .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
+ .div(2),
+ new Rotation2d(
+ MathUtils.modulusMidpoint(
+ estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
+ estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
+ -Math.PI,
+ Math.PI)));
+ }
+
+ // The average translation is just the average of all of the translations (sum divided by total)
+ // Average angle is similar, except every step needs to use a modulus, since -Ï€ is the same
+ // angle as π
+ // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) /
+ // newNumberOfValues
+ Translation2d translation = new Translation2d();
+ double angle = 0;
+ for (int i = 0; i < estimatedPoses.size(); i++) {
+ translation =
+ translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
+ angle =
+ MathUtils.modulusInterpolate(
+ angle,
+ estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(),
+ 1.0 / (i + 1),
+ -Math.PI,
+ Math.PI);
+ }
+
+ return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle));
+ }
+
+ public AprilTagFieldLayout getAprilTagFieldLayout() {
+ return FieldConstants.field;
+ }
+
+ /**
+ * Gets the pose of an april tag
+ *
+ * @param id AprilTag id (1-8)
+ * @return Pose3d of the AprilTag
+ */
+ public Pose3d getTagPose(int id) {
+ if (id < 1 || id > getAprilTagFieldLayout().getTags().size()) {
+ System.out.println("Tried to find the pose of april tag " + id);
+ return null;
+ }
+ return getAprilTagFieldLayout().getTags().get(id - 1).pose;
+ }
+
+ /**
+ * Returns where it thinks the robot is
+ *
+ * @param referencePose The pose to use as a reference, usually the previous robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an april tag
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose) {
+ return getEstimatedPoses(referencePose, ignoree -> referencePose.getRotation().getRadians());
+ }
+
+ /**
+ * Returns where it thinks the robot is
+ *
+ * @param referencePose The pose to use as a reference, usually the previous robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an april tag
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPoses(
+ Pose2d referencePose, DoubleUnaryOperator yawFunction) {
+ ArrayList<EstimatedRobotPose> estimatedPoses = new ArrayList<>();
+ for (int i = 0; i < cameras.size(); i++) {
+ if (VisionConstants.USE_MANUAL_CALCULATIONS) {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)) {
+ if (pose != null) {
+ estimatedPoses.add(pose);
+ }
+ }
+ } else {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)) {
+ // If the camera can see an april tag that exists, add it to the array list
+ // April tags that don't exist might return a result that is present but doesn't have a
+ // pose
+ if (pose.estimatedPose != null) {
+ estimatedPoses.add(pose);
+ }
+ }
+ }
+ }
+ if (estimatedPoses.size() > 1) {
+ Translation2d average = new Translation2d();
+ for (EstimatedRobotPose pose : estimatedPoses) {
+ average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d());
+ }
+ average = average.div(estimatedPoses.size());
+ for (int i = estimatedPoses.size() - 1; i >= 0; i--) {
+ if (estimatedPoses
+ .get(i)
+ .estimatedPose
+ .getTranslation()
+ .toTranslation2d()
+ .getDistance(average)
+ > VisionConstants.MAX_POSE_DIFFERENCE / 2) {
+ estimatedPoses.remove(i);
+ }
+ }
+ }
+ return estimatedPoses;
+ }
+
+ /**
+ * Updates the robot's odometry with vision
+ *
+ * @param poseEstimator The pose estimator to update
+ * @param yawFunction A function that returns the yaw as a double given the timestamp
+ * @param slipped True if the wheels have slipped, false otherwise
+ * @return The list of estimated robot poses from vision
+ */
+ public ArrayList<EstimatedRobotPose> updateOdometry(
+ SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped) {
+ // Simulate vision
+ // 2 ifs to avoid warning
+ if (VisionConstants.ENABLED_SIM) {
+ if (RobotBase.isSimulation()) {
+ visionSim.update(poseEstimator.getEstimatedPosition());
+ }
+ }
+
+ sawTag = false;
+
+ // An array list of poses returned by different cameras
+ ArrayList<EstimatedRobotPose> estimatedPoses =
+ getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
+ for (EstimatedRobotPose estimatedPose : estimatedPoses) {
+ // Continue if this pose doesn't exist
+ if (estimatedPose.timestampSeconds < 0
+ || !onField(estimatedPose.estimatedPose.toPose2d())
+ || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds
+ || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1) {
+ continue;
+ }
+
+ poseEstimator.addVisionMeasurement(
+ estimatedPose.estimatedPose.toPose2d(),
+ estimatedPose.timestampSeconds,
+ slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS);
+ sawTag = true;
+ }
+ return estimatedPoses;
+ }
+
+ /** Updates each camera's inputs for logging */
+ public void updateInputs() {
+ for (VisionCamera c : cameras) {
+ c.updateInputs();
+ }
+ }
+
+ /**
+ * If vision saw any April tags last frame
+ *
+ * @return If vision saw an April tag last frame
+ */
+ public boolean canSeeTag() {
+ return sawTag;
+ }
+
+ /**
+ * Enable or disable a single camera
+ *
+ * @param index The camera index
+ * @param enabled If it should be enabled or disabled
+ */
+ public void enableCamera(int index, boolean enabled) {
+ try {
+ cameras.get(index).enable(enabled);
+ } catch (IndexOutOfBoundsException e) {
+ DriverStation.reportWarning("Camera index " + index + " is out of bounds", false);
+ }
+ }
+
+ /**
+ * Sets the cameras to only use April tag in the specified array
+ *
+ * @param ids The ids of the tags to use, null or empty array to use all
+ */
+ public void onlyUse(int[] ids) {
+ onlyUse = ids;
+ }
+
+ /**
+ * Checks if one or more cameras are disconnected
+ *
+ * @return true if at least one camera is disconnected, false otherwise
+ */
+ public boolean oneCameraDisconnected() {
+ for (VisionCamera camera : cameras) {
+ if (!camera.inputs.connected) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Checks if a pose is on the field
+ *
+ * @param pose The pose to check
+ * @return If the pose is on the field
+ */
+ public static boolean onField(Pose2d pose) {
+ return pose != null
+ && pose.getX() > 0
+ && pose.getX() < FieldConstants.field.getFieldLength()
+ && pose.getY() > 0
+ && pose.getY() < FieldConstants.field.getFieldWidth();
+ }
+
+ /**
+ * Checks if a pose is on or near the field
+ *
+ * @param pose The pose to check
+ * @return If the pose is within an area with twice the length and width of the field
+ */
+ public static boolean nearField(Pose2d pose) {
+ return pose != null
+ && pose.getX() > -FieldConstants.field.getFieldLength() / 2
+ && pose.getX() < FieldConstants.field.getFieldLength() * 1.5
+ && pose.getY() > -FieldConstants.field.getFieldWidth() / 2
+ && pose.getY() < FieldConstants.field.getFieldWidth() * 1.5;
+ }
+
+ private class VisionCamera implements VisionIO {
+ private PhotonCamera camera;
+ private PhotonPoseEstimator photonPoseEstimator;
+ private Pose2d lastPose;
+ private double lastTimestamp = 0;
+ private boolean enabled = true;
+ private final VisionIOInputs inputs = new VisionIOInputs();
+
+ /**
+ * Stores information about a camera
+ *
+ * @param cameraName The name of the camera on PhotonVision
+ * @param robotToCam The transformation from the robot to the camera
+ */
+ public VisionCamera(String cameraName, Transform3d robotToCam) {
+ camera = new PhotonCamera(cameraName);
+ photonPoseEstimator = new PhotonPoseEstimator(FieldConstants.field, robotToCam);
+ lastPose = null;
+ }
+
+ /**
+ * Gets the estimated poses from the camera
+ *
+ * @param referencePose Pose to use for reference, usually the previous estimated robot pose
+ * @return estimated robot poses
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(Pose2d referencePose) {
+
+ ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+ if (!enabled) {
+ return list;
+ }
+
+ for (PhotonPipelineResult cameraResult : inputs.results) {
+ if (!cameraResult.hasTargets() || cameraResult.getTimestampSeconds() < 0) {
+ continue;
+ }
+
+ // if there is a target detected and the timestamp exists,
+ // check the ambiguity isn't too high
+ List<PhotonTrackedTarget> targetsUsed = cameraResult.targets;
+ for (int i = targetsUsed.size() - 1; i >= 0; i--) {
+ // Remove it from the list if it should not be used or if it has too high of an ambiguity
+ if (!useTag(targetsUsed.get(i).getFiducialId())
+ || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY
+ || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm()
+ > VisionConstants.MAX_DISTANCE) {
+ targetsUsed.remove(i);
+ }
+ }
+
+ // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and
+ // the constant is set to only use 2 tags, continue
+ if (targetsUsed.size() == 0
+ || cameraResult.getTimestampSeconds() < 0
+ || targetsUsed.size() == 1 && VisionConstants.ONLY_USE_2_TAGS) {
+ continue;
+ }
+
+ // Set strategy to single tag if there is only 1 good tag and update
+ PhotonPoseEstimator.PoseStrategy poseStrategy =
+ targetsUsed.size() > 1
+ ? VisionConstants.POSE_STRATEGY
+ : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
+ Optional<EstimatedRobotPose> pose;
+ switch (poseStrategy) {
+ case AVERAGE_BEST_TARGETS:
+ pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
+ break;
+ case CLOSEST_TO_CAMERA_HEIGHT:
+ pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
+ break;
+ case CLOSEST_TO_REFERENCE_POSE:
+ pose =
+ photonPoseEstimator.estimateClosestToReferencePose(
+ cameraResult, new Pose3d(referencePose));
+ break;
+ case LOWEST_AMBIGUITY:
+ pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
+ break;
+ case MULTI_TAG_PNP_ON_COPROCESSOR:
+ pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
+ break;
+ case PNP_DISTANCE_TRIG_SOLVE:
+ pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
+ break;
+ case CLOSEST_TO_LAST_POSE:
+ case CONSTRAINED_SOLVEPNP:
+ case MULTI_TAG_PNP_ON_RIO:
+ default:
+ throw new RuntimeException(
+ "Pose estimation method " + poseStrategy.toString() + " is not supported.");
+ }
+
+ if (pose.isPresent()
+ && pose.get() != null
+ && onField(pose.get().estimatedPose.toPose2d())) {
+ double timestamp = cameraResult.getTimestampSeconds();
+
+ // If the pose moved too much, don't use it
+ if (lastPose == null
+ || lastPose
+ .getTranslation()
+ .getDistance(pose.get().estimatedPose.toPose2d().getTranslation())
+ > DriveConstants.MAX_SPEED * 1.25 * (timestamp - lastTimestamp)
+ || timestamp < lastTimestamp) {
+ lastPose = pose.get().estimatedPose.toPose2d();
+ lastTimestamp = timestamp;
+ continue;
+ }
+
+ // Otherwise, add the pose to the list
+ lastPose = pose.get().estimatedPose.toPose2d();
+ lastTimestamp = timestamp;
+ list.add(pose.get());
+ }
+ }
+ return list;
+ }
+
+ /** Updates the VisionIOInputs object with the results from PhotonVision for logging */
+ @Override
+ public void updateInputs() {
+ inputs.connected = camera.isConnected();
+ inputs.results = camera.getAllUnreadResults();
+
+ Logger.processInputs("Vision/" + camera.getName(), inputs);
+ }
+
+ /**
+ * Gets the pose using manual calculations
+ *
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return A list of estimated poses as EstimatedRobotPoses
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction) {
+ ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+ // Do nothing if this camera is disabled
+ if (!enabled) {
+ return list;
+ }
+
+ // The latest camera results
+ for (PhotonPipelineResult result : inputs.results) {
+ // TODO: This could be improved by averaging all targets instead of only using 1
+
+ // Continue if the target doesn't exist or it should be ignored
+ if (!result.hasTargets()) continue;
+ // Gets the best target to use for the calculations
+ PhotonTrackedTarget target = result.getBestTarget();
+ // I don't know why this would happen, but keep it in just in case
+ if (target == null) {
+ continue;
+ }
+ // Continue if the id is too high or too low
+ int id = target.getFiducialId();
+ if (!useTag(id)
+ || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE
+ || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY) {
+ continue;
+ }
+ // Stores target pose and robot to camera transformation for easy access later
+ Pose3d targetPose = FieldConstants.field.getTagPose(id).get();
+ Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform();
+
+ double timestamp = result.getTimestampSeconds();
+ double yaw = yawFunction.applyAsDouble(timestamp);
+
+ // Get the tag position relative to the robot, assuming the robot is on the ground
+ Translation3d translation =
+ target.getBestCameraToTarget().getTranslation().rotateBy(robotToCamera.getRotation());
+ translation =
+ translation // .times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
+ .plus(robotToCamera.getTranslation())
+ .rotateBy(new Rotation3d(0, 0, yaw))
+
+ // Invert it to get the robot position relative to the April tag
+ // Multiply by a constant. I don't know why this works, but it was consistently 10%
+ // off in 2023 Fall Semester
+ .times(-VisionConstants.DISTANCE_SCALE)
+ // Get the field relative robot pose
+ .plus(targetPose.getTranslation());
+ try {
+ // Adds an EstimatedRobotPose
+ list.add(
+ new EstimatedRobotPose(
+ new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
+ timestamp,
+ List.of(target),
+ VisionConstants.POSE_STRATEGY));
+ } catch (Exception e) {
+ DriverStation.reportError("Error creating EstimatedRobotPose", true);
+ }
+ }
+ return list;
+ }
+
+ public boolean useTag(int id) {
+ // Never use tags that don't exist
+ if (id <= 0 || id > FieldConstants.field.getTags().size()) {
+ return false;
+ }
+ // Return false if it is in the list of tags to ignore
+ for (int id2 : VisionConstants.TAGS_TO_IGNORE) {
+ if (id == id2) {
+ return false;
+ }
+ }
+ // If it's in the array to only use and not in the array to ignore, return true
+ for (int j = 0; onlyUse != null && j < onlyUse.length; j++) {
+ if (id == onlyUse[j]) {
+ return true;
+ }
+ }
+ // If it isn't in the array to only use, only reutrn true if the array is empty/null
+ return onlyUse == null || onlyUse.length == 0;
+ }
+
+ /**
+ * Enables or disables this camera
+ *
+ * @param enable If it should be enabled or disabled
+ */
+ public void enable(boolean enable) {
+ enabled = enable;
+ }
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+
+public interface VisionIO {
+ public static class VisionIOInputs implements LoggableInputs {
+ public boolean connected = false;
+ public List<PhotonPipelineResult> results = new ArrayList<>();
+
+ // PhotonVision should never return more than 5 results, except possibly for very long loop
+ // overruns
+ private static final int maxLength = 5;
+
+ private boolean intitalized = false;
+
+ @Override
+ public void toLog(LogTable table) {
+ // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra
+ // values will need to be ignored
+ // This is not very efficient, since unused values are still taking up memory, but there is no
+ // easy way to remove them
+ if (!intitalized) {
+ for (int i = 0; i < maxLength; i++) {
+ table.put("Results" + i, new PhotonPipelineResult());
+ intitalized = true;
+ }
+ }
+ table.put("Connected", connected);
+ double length = Math.min(results.size(), maxLength);
+ table.put("Length", length);
+ for (int i = 0; i < length; i++) {
+ table.put("Results" + i, results.get(i));
+ }
+ }
+
+ @Override
+ public void fromLog(LogTable table) {
+ connected = table.get("Connected", false);
+ int length = table.get("Length", 0);
+ results = new ArrayList<>(length);
+ // Java gets confused when null is used for a generic type argument
+ PhotonPipelineResult nullResult = null;
+ for (int i = 0; i < length; i++) {
+ PhotonPipelineResult result = table.get("Results" + i, nullResult);
+ if (result != null) {
+ results.add(result);
+ }
+ }
+ }
+ }
+
+ /** Represents the angle to a simple target, not used for pose estimation. */
+ public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+
+ /** Represents a robot pose sample used for pose estimation. */
+ public static record PoseObservation(
+ double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance) {}
+
+ public default void updateInputs() {}
+}
--- /dev/null
+package lib;
+
+import org.wpilib.math.util.Units;
+
+/* Contains values and required settings for common COTS swerve modules. */
+public class COTSFalconSwerveConstants {
+ public final double wheelDiameter;
+ public final double wheelCircumference;
+ public final double angleGearRatio;
+ public final double driveGearRatio;
+ public final double angleKP;
+ public final double angleKI;
+ public final double angleKD;
+ public final double angleKF;
+ public final boolean driveMotorInvert;
+ public final boolean angleMotorInvert;
+ public final boolean canCoderInvert;
+
+ public COTSFalconSwerveConstants(
+ double wheelDiameter,
+ double angleGearRatio,
+ double driveGearRatio,
+ double angleKP,
+ double angleKI,
+ double angleKD,
+ double angleKF,
+ boolean driveMotorInvert,
+ boolean angleMotorInvert,
+ boolean canCoderInvert) {
+ this.wheelDiameter = wheelDiameter;
+ this.wheelCircumference = wheelDiameter * Math.PI;
+ this.angleGearRatio = angleGearRatio;
+ this.driveGearRatio = driveGearRatio;
+ this.angleKP = angleKP;
+ this.angleKI = angleKI;
+ this.angleKD = angleKD;
+ this.angleKF = angleKF;
+ this.driveMotorInvert = driveMotorInvert;
+ this.angleMotorInvert = angleMotorInvert;
+ this.canCoderInvert = canCoderInvert;
+ }
+
+ /** Swerve Drive Specialties - MK3 Module */
+ public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK4 Module */
+ public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK4i Module */
+ public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (150 / 7) : 1 */
+ double angleGearRatio = ((150.0 / 7.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK5n Module */
+ public static COTSFalconSwerveConstants SDSMK5n(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (287 / 11) : 1 */
+ double angleGearRatio = ((287.0 / 11.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /* Drive Gear Ratios for all supported modules */
+ public static class DriveGearRatios {
+ /* SDS MK3 */
+ /** SDS MK3 - 8.16 : 1 */
+ public static final double SDSMK3_Standard = (8.16);
+
+ /** SDS MK3 - 6.86 : 1 */
+ public static final double SDSMK3_Fast = (6.86);
+
+ /* SDS MK4 */
+ /** SDS MK4 - 8.14 : 1 */
+ public static final double SDSMK4_L1 = (8.14);
+
+ /** SDS MK4 - 6.75 : 1 */
+ public static final double SDSMK4_L2 = (6.75);
+
+ /** SDS MK4 - 6.12 : 1 */
+ public static final double SDSMK4_L3 = (6.12);
+
+ /** SDS MK4 - 5.14 : 1 */
+ public static final double SDSMK4_L4 = (5.14);
+
+ /* SDS MK4i */
+ /** SDS MK4i - 8.14 : 1 */
+ public static final double SDSMK4i_L1 = (8.14);
+
+ /** SDS MK4i - 6.75 : 1 */
+ public static final double SDSMK4i_L2 = (6.75);
+
+ /** SDS MK4i - 6.12 : 1 */
+ public static final double SDSMK4i_L3 = (6.12);
+
+ /* SDS MK5n */
+ /** SDS MK5n - 7.13 : 1 */
+ public static final double SDSMK5n_L1_PLUS = (8.13);
+
+ /** SDS MK4i - 5.9 : 1 */
+ public static final double SDSMK5n_L2_PLUS = (5.9);
+
+ /** SDS MK4i - 5.36 : 1 */
+ public static final double SDSMK5n_L3_PLUS = (5.35);
+ }
+}
--- /dev/null
+package lib;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModuleState;
+
+public class CTREModuleState {
+
+ /**
+ * Minimize the change in heading the desired swerve module state would require by potentially
+ * reversing the direction the wheel spins. Customized from WPILib's version to include placing in
+ * appropriate scope for CTRE onboard control.
+ *
+ * @param desiredState The desired state.
+ * @param currentAngle The current module angle.
+ */
+ public static SwerveModuleState optimize(
+ SwerveModuleState desiredState, Rotation2d currentAngle) {
+ double targetAngle =
+ placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+ double targetSpeed = desiredState.speedMetersPerSecond;
+ double delta = targetAngle - currentAngle.getDegrees();
+ if (Math.abs(delta) > 90) {
+ targetSpeed = -targetSpeed;
+ if (delta > 90) {
+ targetAngle -= 180;
+ } else {
+ targetAngle += 180;
+ }
+ }
+ return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+ }
+
+ /**
+ * @param scopeReference Current Angle
+ * @param newAngle Target Angle
+ * @return Closest angle within scope
+ */
+ private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+ double lowerBound;
+ double upperBound;
+ double lowerOffset = scopeReference % 360;
+ if (lowerOffset >= 0) {
+ lowerBound = scopeReference - lowerOffset;
+ upperBound = scopeReference + (360 - lowerOffset);
+ } else {
+ upperBound = scopeReference - lowerOffset;
+ lowerBound = scopeReference - (360 + lowerOffset);
+ }
+ while (newAngle < lowerBound) {
+ newAngle += 360;
+ }
+ while (newAngle > upperBound) {
+ newAngle -= 360;
+ }
+ if (newAngle - scopeReference > 180) {
+ newAngle -= 360;
+ } else if (newAngle - scopeReference < -180) {
+ newAngle += 360;
+ }
+ return newAngle;
+ }
+}
--- /dev/null
+package lib;
+
+import Jama.Matrix;
+import Jama.QRDecomposition;
+
+// NOTE: This file is available at
+// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
+
+/**
+ * The {@code PolynomialRegression} class performs a polynomial regression on an set of <em>N</em>
+ * data points (<em>y<sub>i</sub></em>, <em>x<sub>i</sub></em>). That is, it fits a polynomial
+ * <em>y</em> = β<sub>0</sub> + β<sub>1</sub> <em>x</em> + β<sub>2</sub>
+ * <em>x</em><sup>2</sup> + ... + β<sub><em>d</em></sub> <em>x</em><sup><em>d</em></sup> (where
+ * <em>y</em> is the response variable, <em>x</em> is the predictor variable, and the
+ * β<sub><em>i</em></sub> are the regression coefficients) that minimizes the sum of squared
+ * residuals of the multiple regression model. It also computes associated the coefficient of
+ * determination <em>R</em><sup>2</sup>.
+ *
+ * <p>This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is
+ * neither the fastest nor the most numerically stable way to perform the polynomial regression.
+ *
+ * @author Robert Sedgewick
+ * @author Kevin Wayne
+ */
+public class PolynomialRegression implements Comparable<PolynomialRegression> {
+ private final String variableName; // name of the predictor variable
+ private int degree; // degree of the polynomial regression
+ private final Matrix beta; // the polynomial regression coefficients
+ private final double sse; // sum of squares due to error
+ private double sst; // total sum of squares
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name of
+ * the predictor variable.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree) {
+ this(x, y, degree, "n");
+ }
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @param variableName the name of the predictor variable
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree, String variableName) {
+ this.degree = degree;
+ this.variableName = variableName;
+
+ int n = x.length;
+ QRDecomposition qr = null;
+ Matrix matrixX = null;
+
+ // in case Vandermonde matrix does not have full rank, reduce degree until it
+ // does
+ while (true) {
+
+ // build Vandermonde matrix
+ double[][] vandermonde = new double[n][this.degree + 1];
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j <= this.degree; j++) {
+ vandermonde[i][j] = Math.pow(x[i], j);
+ }
+ }
+ matrixX = new Matrix(vandermonde);
+
+ // find least squares solution
+ qr = new QRDecomposition(matrixX);
+ if (qr.isFullRank()) break;
+
+ // decrease degree and try again
+ this.degree--;
+ }
+
+ // create matrix from vector
+ Matrix matrixY = new Matrix(y, n);
+
+ // linear regression coefficients
+ beta = qr.solve(matrixY);
+
+ // mean of y[] values
+ double sum = 0.0;
+ for (int i = 0; i < n; i++) sum += y[i];
+ double mean = sum / n;
+
+ // total variation to be accounted for
+ for (int i = 0; i < n; i++) {
+ double dev = y[i] - mean;
+ sst += dev * dev;
+ }
+
+ // variation not accounted for
+ Matrix residuals = matrixX.times(beta).minus(matrixY);
+ sse = residuals.norm2() * residuals.norm2();
+ }
+
+ /**
+ * Returns the {@code j}th regression coefficient.
+ *
+ * @param j the index
+ * @return the {@code j}th regression coefficient
+ */
+ public double beta(int j) {
+ // to make -0.0 print as 0.0
+ if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
+ return beta.get(j, 0);
+ }
+
+ /**
+ * Returns the degree of the polynomial to fit.
+ *
+ * @return the degree of the polynomial to fit
+ */
+ public int degree() {
+ return degree;
+ }
+
+ /**
+ * Returns the coefficient of determination <em>R</em><sup>2</sup>.
+ *
+ * @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
+ * 0 and 1
+ */
+ public double R2() {
+ if (sst == 0.0) return 1.0; // constant function
+ return 1.0 - sse / sst;
+ }
+
+ /**
+ * Returns the expected response {@code y} given the value of the predictor variable {@code x}.
+ *
+ * @param x the value of the predictor variable
+ * @return the expected response {@code y} given the value of the predictor variable {@code x}
+ */
+ public double predict(double x) {
+ // horner's method
+ double y = 0.0;
+ for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
+ return y;
+ }
+
+ /**
+ * Returns a string representation of the polynomial regression model.
+ *
+ * @return a string representation of the polynomial regression model, including the best-fit
+ * polynomial and the coefficient of determination <em>R</em><sup>2</sup>
+ */
+ public String toString() {
+ StringBuilder s = new StringBuilder();
+ int j = degree;
+
+ // ignoring leading zero coefficients
+ while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;
+
+ // create remaining terms
+ while (j >= 0) {
+ if (j == 0) s.append(String.format("%.4f ", beta(j)));
+ else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName));
+ else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j));
+ j--;
+ }
+ s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
+
+ // replace "+ -2n" with "- 2n"
+ return s.toString().replace("+ -", "- ");
+ }
+
+ /** Compare lexicographically. */
+ public int compareTo(PolynomialRegression that) {
+ double EPSILON = 1E-5;
+ int maxDegree = Math.max(this.degree(), that.degree());
+ for (int j = maxDegree; j >= 0; j--) {
+ double term1 = 0.0;
+ double term2 = 0.0;
+ if (this.degree() >= j) term1 = this.beta(j);
+ if (that.degree() >= j) term2 = that.beta(j);
+ if (Math.abs(term1) < EPSILON) term1 = 0.0;
+ if (Math.abs(term2) < EPSILON) term2 = 0.0;
+ if (term1 < term2) return -1;
+ else if (term1 > term2) return +1;
+ }
+ return 0;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class Controller {
+ protected final Joystick controller;
+
+ public Controller(int port) {
+ this.controller = new Joystick(port);
+ }
+
+ public Trigger get(BooleanSupplier sup) {
+ return new Trigger(sup);
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+public class Ex3DProController extends Controller {
+ public Ex3DProController(int port) {
+ super(port);
+ }
+
+ public enum Ex3DProButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7),
+ B8(8),
+ B9(9),
+ B10(10),
+ B11(11),
+ B12(12);
+
+ public final int id;
+
+ Ex3DProButton(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Ex3DProAxis {
+ X(0),
+ Y(1),
+ Z(2),
+ SLIDER(3);
+
+ public final int id;
+
+ Ex3DProAxis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Ex3DProHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ Ex3DProHatSwitch(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(Ex3DProButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Ex3DProAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(Ex3DProHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.GenericHID.RumbleType;
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class GameController extends Controller {
+ // These are the different controller triggers
+ public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
+ RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+ public final BooleanSupplier LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75;
+
+ public GameController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ A(1),
+ B(2),
+ X(3),
+ Y(4),
+ LB(5),
+ RB(6),
+ BACK(7),
+ START(8),
+ LEFT_JOY(9),
+ RIGHT_JOY(10);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ LEFT_TRIGGER(2),
+ RIGHT_TRIGGER(3),
+ RIGHT_X(4),
+ RIGHT_Y(5);
+
+ public final int id;
+
+ Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public enum RumbleStatus {
+ RUMBLE_ON(0.7),
+ RUMBLE_OFF(0);
+
+ public final double rumbleValue;
+
+ RumbleStatus(final double rumbleValue) {
+ this.rumbleValue = rumbleValue;
+ }
+ }
+
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+
+ public void setRumble(RumbleStatus rumbleStatus) {
+ controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
+ controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+public class MadCatzController extends Controller {
+ public final Trigger
+ ALL_UP =
+ get(MadCatzHatSwitch.UP)
+ .or(get(MadCatzHatSwitch.UP_LEFT))
+ .or(get(MadCatzHatSwitch.UP_RIGHT)),
+ ALL_DOWN =
+ get(MadCatzHatSwitch.DOWN)
+ .or(get(MadCatzHatSwitch.DOWN_LEFT))
+ .or(get(MadCatzHatSwitch.DOWN_RIGHT)),
+ ALL_LEFT =
+ get(MadCatzHatSwitch.LEFT)
+ .or(get(MadCatzHatSwitch.UP_LEFT))
+ .or(get(MadCatzHatSwitch.DOWN_LEFT)),
+ ALL_RIGHT =
+ get(MadCatzHatSwitch.RIGHT)
+ .or(get(MadCatzHatSwitch.UP_RIGHT))
+ .or(get(MadCatzHatSwitch.DOWN_RIGHT));
+
+ public MadCatzController(int port) {
+ super(port);
+ }
+
+ public enum MadCatzButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7);
+
+ public final int id;
+
+ MadCatzButton(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum MadCatzAxis {
+ X(0),
+ Y(1),
+ SLIDER(2),
+ ZROTATE(3);
+
+ public final int id;
+
+ MadCatzAxis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum MadCatzHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ MadCatzHatSwitch(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(MadCatzButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(MadCatzAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(MadCatzHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class PS5Controller extends Controller {
+ // These are the different controller triggers
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+ public final BooleanSupplier LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75;
+
+ public PS5Controller(int port) {
+ super(port);
+ }
+
+ public enum PS5Button {
+ SQUARE(1),
+ CROSS(2),
+ CIRCLE(3),
+ TRIANGLE(4),
+ LB(5),
+ RB(6),
+ LEFT_TRIGGER(7),
+ RIGHT_TRIGGER(8),
+ CREATE(9),
+ OPTIONS(10),
+ LEFT_JOY(11),
+ RIGHT_JOY(12),
+ PS(13),
+ TOUCHPAD(14),
+ MUTE(15);
+
+ public final int id;
+
+ PS5Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum PS5Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ RIGHT_X(2),
+ /** note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed */
+ LEFT_TRIGGER(3),
+ /** note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed */
+ RIGHT_TRIGGER(4),
+ RIGHT_Y(5);
+
+ public final int id;
+
+ PS5Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(PS5Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(PS5Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import org.wpilib.driverstation.Joystick;
+import org.wpilib.command2.button.Trigger;
+
+public class PistolController extends Controller {
+ public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
+ TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
+ BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
+ BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
+
+ public PistolController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ TOP_BACK(1),
+ TOP_FRONT(2),
+ BOTTOM_FRONT(3),
+ BOTTOM_BACK(4),
+ BOTTOM(5);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Axis {
+ WHEEL(0),
+ TRIGGER(1);
+
+ public final int id;
+
+ Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import org.wpilib.vision.apriltag.AprilTag;
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.math.util.Pair;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import frc.robot.util.Vision.Vision;
+
+/** Tests if all of the AprilTags are in the right spot */
+public class AprilTagPoseTest {
+ /**
+ * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in
+ * FieldConstants, and that they are on the right side of the field
+ */
+ @Test
+ public void testTagPoses() {
+ // Construct the vision instance
+ // makes the field layout
+ Vision vision = new Vision(new ArrayList<Pair<String, Transform3d>>());
+
+ // we should have 32 tags
+ assertEquals(32, FieldConstants.field.getTags().size());
+ assertEquals(32, vision.getAprilTagFieldLayout().getTags().size());
+
+ // Check each tag in the field layout
+ for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) {
+ // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is
+ // i+1.
+ int tagId = i + 1;
+
+ // Get the poses from the two sources
+ // From the ArrayList<AprilTag> source
+ AprilTag apriltag1 = FieldConstants.field.getTags().get(i);
+ Pose3d p1 = apriltag1.pose;
+ // From the vision source
+ Pose3d p2 = vision.getTagPose(tagId);
+
+ // Check the tag id in the ArrayList
+ assertEquals(tagId, apriltag1.ID);
+
+ // Make sure the points match
+ assertEquals(p1.getX(), p2.getX(), 0.0001);
+ assertEquals(p1.getY(), p2.getY(), 0.0001);
+ assertEquals(p1.getZ(), p2.getZ(), 0.0001);
+
+ // Make sure the rotations match
+ assertEquals(p1.getRotation().getX(), p2.getRotation().getX(), 0.0001);
+ assertEquals(p1.getRotation().getY(), p2.getRotation().getY(), 0.0001);
+ assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001);
+
+ // 1-16 should be on the right, and 17-36 should be on the left
+ if (tagId > 16) {
+ assertTrue(p1.getX() < FieldConstants.field.getFieldLength() / 2);
+ } else {
+ assertTrue(p1.getX() > FieldConstants.field.getFieldLength() / 2);
+ }
+ }
+ }
+
+ @Test
+ public void testReefTags() {
+ List<Pose3d> redPoses =
+ FieldConstants.field.getTags().subList(1, 16).stream().map(tag -> tag.pose).toList();
+ List<Pose3d> bluePoses =
+ FieldConstants.field.getTags().subList(17, 32).stream().map(tag -> tag.pose).toList();
+ Pose3d redCenter = findCenter(redPoses);
+ Pose3d blueCenter = findCenter(bluePoses);
+
+ // The tags should be symmetrical, so the total rotation should be 0
+ assertEquals(redCenter.getRotation().getX(), 0, 0.0001);
+ assertEquals(blueCenter.getRotation().getX(), 0, 0.0001);
+ assertEquals(redCenter.getRotation().getY(), 0, 0.0001);
+ assertEquals(blueCenter.getRotation().getY(), 0, 0.0001);
+ assertEquals(MathUtil.angleModulus(redCenter.getRotation().getZ()), Math.PI, 0.0001);
+ assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), 0, 0.0001);
+
+ // Y are symmetrical diagonally
+ assertEquals(redCenter.getY(), FieldConstants.field.getFieldWidth() - blueCenter.getY(), 0.01);
+ // Z should be equal
+ assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001);
+
+ // X should be mirrored
+ assertEquals(redCenter.getX(), FieldConstants.field.getFieldLength() - blueCenter.getX(), 0.01);
+
+ // Compare each matching pair of tags
+ for (int i = 1; i < 17; i++) {
+ Pose3d red = FieldConstants.field.getTagPose(i).get();
+ Pose3d blue = FieldConstants.field.getTagPose(i + 16).get();
+ assertEquals(red.getY(), FieldConstants.field.getFieldWidth() - blue.getY(), 0.01);
+ assertEquals(red.getZ(), blue.getZ(), 0.0001);
+ assertEquals(red.getX(), FieldConstants.field.getFieldLength() - blue.getX(), 0.01);
+ assertEquals(
+ MathUtil.angleModulus(red.getRotation().getZ()),
+ MathUtil.angleModulus(blue.getRotation().getZ() + Math.PI),
+ 0.0001);
+ }
+ }
+
+ /**
+ * Gets the center pose with the sum of the rotations, used for checking the reef
+ *
+ * @param poses The poses to find the center of
+ * @return A pose with the translation at the center and the sum of the rotations
+ */
+ private Pose3d findCenter(List<Pose3d> poses) {
+ double x = 0;
+ double y = 0;
+ double z = 0;
+ Rotation3d rot = new Rotation3d();
+ for (Pose3d pose : poses) {
+ x += pose.getX();
+ y += pose.getY();
+ z += pose.getZ();
+ rot = rot.plus(pose.getRotation());
+ }
+ x /= poses.size();
+ y /= poses.size();
+ z /= poses.size();
+ return new Pose3d(x, y, z, rot);
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import org.wpilib.math.util.Units;
+import frc.robot.constants.swerve.DriveConstants;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/** Check some robot constants/parameters. */
+public class ConstantsTest {
+
+ @Test
+ public void testRobotSize() {
+ // The competition robot frame width and length is 26 inches.
+ // It has 3/16 inch plates on all sides,
+ // so frame width is 26.375!
+ double widthFrame = Units.inchesToMeters(26.0);
+ double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0);
+
+ // At Port Hueneme, the frame perimeter was 105 inches
+ // frame perimeter is 105.5...
+ assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501);
+
+ // Bumpers.
+ // The backing board is 0.75 inches.
+ // The noodles are 2.5 inches.
+ // Board + noodles = 3.25 inches
+ // Measure the red bumpers, and they are 3.5 inches.
+ //
+ // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches
+ // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the
+ // bumpers.
+ // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame
+ double thickBumpers = Units.inchesToMeters(3.5 + 0.25);
+
+ // so width with bumpers is
+ @SuppressWarnings("unused")
+ double widthFrameWithBumpers = widthFrame + 2 * thickBumpers;
+
+ // check with values in DriveConstants
+ // System.out.printf("widthFrameWithBumpers = %8f %8f\n", widthFrameWithBumpers,
+ // Units.metersToInches(widthFrameWithBumpers));
+ // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n",
+ // DriveConstants.kRobotWidthWithBumpers,
+ // Units.metersToInches(DriveConstants.kRobotWidthWithBumpers));
+ // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001);
+ }
+
+ /** MK4i module https://www.swervedrivespecialties.com/products/mk4i-swerve-module */
+ enum SwerveDriveSpecialties {
+ // Gearbox ratios from the SDS webpage
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)),
+ // 16-tooth pinion
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0));
+
+ /** Drive gear ratio varies for each module */
+ final double driveRatio;
+
+ /** Steering Gear ratio (same for all MK4i modules) */
+ final double steerRatio = 150.0 / 7.0;
+
+ SwerveDriveSpecialties(double drive) {
+ this.driveRatio = drive;
+ }
+ }
+
+ @Test
+ public void testSwerveRatios() {
+ // check the mroe exact ratios against the published-to-2-digits ratios
+ assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01);
+ assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01);
+ assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01);
+
+ // The drive ratio could be more accurate, but does not hurt
+ assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01);
+
+ // The steer ratio
+ // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about
+ // 1.5 degrees)
+ // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) /
+ // SwerveDriveSpecialties.L2.steerRatio);
+ assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Example of a JUnit test class. This test should run everytime someone builds the robot code. See
+ * https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html
+ *
+ * <p>To disable a test, annotate with Disabled
+ */
+public class ArithTest {
+
+ @BeforeEach
+ public void prepare() {}
+
+ @AfterEach
+ public void cleanup() {}
+
+ /** Test if floating point addition works. */
+ @Test
+ public void testSimpleArith() {
+ assertEquals(5.0, 2.0 + 3.0, 0.0001);
+ }
+
+ /** Here is a disabled test */
+ @Disabled
+ @Test
+ public void testNaught() {
+ assertEquals(0, 0);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.Random;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.DetectedObject.ObjectType;
+
+/** Tests DetectedObject */
+public class DetectedObjectTest {
+
+ @BeforeEach
+ public void prepare() {
+ DetectedObject.setDrive(null);
+ }
+
+ @AfterEach
+ public void cleanup() {}
+
+ /** Tests if the objec pose is correct */
+ @Test
+ public void testObjectPose() {
+ DetectedObject object =
+ new DetectedObject(
+ Units.degreesToRadians(45),
+ 0,
+ 1,
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 2, Math.PI / 2)));
+ Translation3d expected = new Translation3d(Math.sqrt(2) / 2, 0, Math.sqrt(2) / 2 + 1);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ }
+
+ /** Tests the position of an object when distance is not specified */
+ @Test
+ public void testObjectPoseWithoutDistance() {
+ DetectedObject object =
+ new DetectedObject(
+ 0,
+ -Units.degreesToRadians(20),
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(0, 0, 1), new Rotation3d(0, Units.degreesToRadians(25), 0)));
+ Translation3d expected = new Translation3d(1, 0, 0);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ }
+
+ /** Tests if the object is on the ground using random offsets */
+ @Test
+ public void testObjectOnGround() {
+ Random random = new Random();
+ DetectedObject object =
+ new DetectedObject(
+ random.nextDouble(-Math.PI, Math.PI),
+ random.nextDouble(0.001, Math.PI / 4),
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(
+ random.nextDouble(0, 100),
+ random.nextDouble(0, 100),
+ random.nextDouble(0.1, 100)),
+ new Rotation3d(
+ 0,
+ random.nextDouble(0.001, Math.PI / 4),
+ random.nextDouble(-Math.PI, Math.PI))));
+ assertEquals(object.pose.getZ(), 0, 0.001);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+
+import com.pathplanner.lib.auto.NamedCommands;
+
+import org.wpilib.command2.InstantCommand;
+
+/** Simple check on PathPlanner path */
+public class PathCheck {
+
+ /**
+ * Register placeholder commands for testing. These are normally registered in
+ * RobotContainer.registerCommands(), but the test doesn't instantiate RobotContainer.
+ */
+ @BeforeAll
+ public static void registerPlaceholderCommands() {
+ NamedCommands.registerCommand("Extend intake", new InstantCommand());
+ NamedCommands.registerCommand("Intake", new InstantCommand());
+ }
+
+ /**
+ * Load the path groups.
+ *
+ * <p>We have had problems with syntax errors in a path.
+ */
+ @Test
+ public void pathGroupLoaderTest() {
+ // load the paths
+ // may throw a ParseException; that error will fail this test
+ PathGroupLoader.loadPathGroups();
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class PolynomialRegressionTest {
+
+ /** Unit tests the {@code PolynomialRegression} data type. */
+ @Test
+ public void testRegression() {
+ double[] x = {10, 20, 40, 80, 160, 200};
+ double[] y = {100, 350, 1500, 6700, 20160, 40000};
+ PolynomialRegression regression = new PolynomialRegression(x, y, 3);
+
+ assertEquals(regression.beta(3), 0.0092, 0.0001);
+ assertEquals(regression.beta(2), -1.6395, 0.0001);
+ assertEquals(regression.beta(1), 168.9232, 0.0001);
+ assertEquals(regression.beta(0), -2113.7306, 0.0001);
+ }
+}
--- /dev/null
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "27.0.0-alpha-4",
+ "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+ "wpilibYear": "2027_alpha5",
+ "mavenUrls": [
+ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+ ],
+ "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-java",
+ "version": "27.0.0-alpha-4"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-wpilibio",
+ "version": "27.0.0-alpha-4",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxsystemcore",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "CommandsV2.json",
+ "name": "Commands V2",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "wpilibYear": "2027_alpha5",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "conflictsWith": [
+ {
+ "uuid": "4decdc05-a056-46cf-9561-39449bbb01306",
+ "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.",
+ "offlineFileName": "CommandsV3.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.wpilib.commandsv2",
+ "artifactId": "commandsv2-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "org.wpilib.commandsv2",
+ "artifactId": "commandsv2-cpp",
+ "version": "wpilib",
+ "libName": "commandsv2",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxsystemcore",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
--- /dev/null
+{
+ "fileName": "PathplannerLibSystemCoreAlpha.json",
+ "name": "PathplannerLib",
+ "version": "2027.0.0-alpha-3",
+ "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+ "wpilibYear": "2027_alpha5",
+ "mavenUrls": [
+ "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+ ],
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLibSystemCoreAlpha.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-java",
+ "version": "2027.0.0-alpha-3"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-cpp",
+ "version": "2027.0.0-alpha-3",
+ "libName": "PathplannerLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxsystemcore",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "Phoenix6-26.50.0-alpha-1.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "26.50.0-alpha-1",
+ "wpilibYear": "2027_alpha5",
+ "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-frc2027-latest.json",
+ "conflictsWith": [
+ {
+ "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+ "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix6-replay-frc2027-latest.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-java",
+ "version": "26.50.0-alpha-1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "api-cpp",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxsystemcore"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxsystemcore"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFXS",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANrange",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdi",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdle",
+ "version": "26.50.0-alpha-1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "26.50.0-alpha-1",
+ "libName": "CTRE_Phoenix6_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxsystemcore"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.50.0-alpha-1",
+ "libName": "CTRE_PhoenixTools",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxsystemcore"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "26.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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.50.0-alpha-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
--- /dev/null
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2027.0.0-alpha-6",
+ "wpilibYear": "2027_alpha5",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2027.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2027.0.0-alpha-6"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2027.0.0-alpha-6",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibBackendDriver",
+ "version": "2027.0.0-alpha-6",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibWpiBackendDriver",
+ "version": "2027.0.0-alpha-6",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2027.0.0-alpha-6",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2027.0.0-alpha-6",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibBackendDriver",
+ "version": "2027.0.0-alpha-6",
+ "libName": "BackendDriver",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibWpiBackendDriver",
+ "version": "2027.0.0-alpha-6",
+ "libName": "REVLibWpi",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxsystemcore",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2027.0.0-alpha-2",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+ "wpilibYear": "2027_alpha5",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+ "jniDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2027.0.0-alpha-2",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxsystemcore",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-cpp",
+ "version": "v2027.0.0-alpha-2",
+ "libName": "photonlib",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxsystemcore",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2027.0.0-alpha-2",
+ "libName": "photontargeting",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxsystemcore",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-java",
+ "version": "v2027.0.0-alpha-2"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-java",
+ "version": "v2027.0.0-alpha-2"
+ }
+ ]
+}
\ No newline at end of file