From: iefomit <108955303+iefomit@users.noreply.github.com> Date: Sun, 23 Aug 2026 16:52:44 +0000 (-0700) Subject: Initial commit X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3d79d9f2840ab5f36d564a4f6bb68e9ed1cb0211;p=FRC2027.git Initial commit --- 3d79d9f2840ab5f36d564a4f6bb68e9ed1cb0211 diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..34cbaac --- /dev/null +++ b/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..3c5c2e6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,61 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "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 +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..67355ff --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2027_alpha5", + "teamNumber": 972 +} \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..eb3061b --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +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. diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..55508d0 --- /dev/null +++ b/build.gradle @@ -0,0 +1,111 @@ +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' +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..d997cfc Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..42bb5ac --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-9.4.1-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 0000000..739907d --- /dev/null +++ b/gradlew @@ -0,0 +1,248 @@ +#!/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" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..c4bdd3a --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,93 @@ +@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 diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..e2626a8 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,30 @@ +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"); diff --git a/src/main/deploy/chirp/file.chrp b/src/main/deploy/chirp/file.chrp new file mode 100644 index 0000000..8e0a95d Binary files /dev/null and b/src/main/deploy/chirp/file.chrp differ diff --git a/src/main/deploy/choreo/project.chor b/src/main/deploy/choreo/project.chor new file mode 100644 index 0000000..de587e0 --- /dev/null +++ b/src/main/deploy/choreo/project.chor @@ -0,0 +1,99 @@ +{ + "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 + } +} diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..82fce05 --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,175 @@ +{ + "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 diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..b679de4 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,46 @@ +{ + "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 diff --git a/src/main/deploy/sshpass b/src/main/deploy/sshpass new file mode 100644 index 0000000..cb02cb5 Binary files /dev/null and b/src/main/deploy/sshpass differ diff --git a/src/main/java/first/Main.java b/src/main/java/first/Main.java new file mode 100644 index 0000000..8fd1dab --- /dev/null +++ b/src/main/java/first/Main.java @@ -0,0 +1,25 @@ +// 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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(frc.robot.Robot.class); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..4218cf7 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,203 @@ +// 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. + * + *

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 dsAlliance = DriverStation.getAlliance(); + if (dsAlliance.isPresent()) return dsAlliance.get(); + else return Alliance.Red; // default to Red alliance + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..26d35a7 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,287 @@ +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 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. + * + *

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() {} +} diff --git a/src/main/java/frc/robot/RobotId.java b/src/main/java/frc/robot/RobotId.java new file mode 100644 index 0000000..8be9b29 --- /dev/null +++ b/src/main/java/frc/robot/RobotId.java @@ -0,0 +1,120 @@ +package frc.robot; + +import org.wpilib.util.Preferences; + +/** + * Set of known Robot Names. + * + *

The name of a robot in the RoboRIO's persistent memory. At deploy time, that name is used to + * set the corresponding RobotId. + * + *

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). + * + *

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()); + } +} diff --git a/src/main/java/frc/robot/commands/DoNothing.java b/src/main/java/frc/robot/commands/DoNothing.java new file mode 100644 index 0000000..384f409 --- /dev/null +++ b/src/main/java/frc/robot/commands/DoNothing.java @@ -0,0 +1,6 @@ +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 {} diff --git a/src/main/java/frc/robot/commands/LogCommand.java b/src/main/java/frc/robot/commands/LogCommand.java new file mode 100644 index 0000000..9456eda --- /dev/null +++ b/src/main/java/frc/robot/commands/LogCommand.java @@ -0,0 +1,29 @@ +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; + } +} diff --git a/src/main/java/frc/robot/commands/Music.java b/src/main/java/frc/robot/commands/Music.java new file mode 100644 index 0000000..c16a780 --- /dev/null +++ b/src/main/java/frc/robot/commands/Music.java @@ -0,0 +1,39 @@ +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; + } +} diff --git a/src/main/java/frc/robot/commands/SupplierCommand.java b/src/main/java/frc/robot/commands/SupplierCommand.java new file mode 100644 index 0000000..3005027 --- /dev/null +++ b/src/main/java/frc/robot/commands/SupplierCommand.java @@ -0,0 +1,51 @@ +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 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 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(); + } +} diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java new file mode 100644 index 0000000..888e1a1 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java @@ -0,0 +1,21 @@ +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); + } +} diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java new file mode 100644 index 0000000..517ea16 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -0,0 +1,21 @@ +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() + // ); + // } +} diff --git a/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java new file mode 100644 index 0000000..eb6c7bd --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java @@ -0,0 +1,52 @@ +// 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())); + } + } + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java new file mode 100644 index 0000000..5ddaa9a --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -0,0 +1,81 @@ +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); + } + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java new file mode 100644 index 0000000..d2331db --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java @@ -0,0 +1,213 @@ +// 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 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 robot; + + private Supplier linearFF = () -> Translation2d.kZero; + private DoubleSupplier omegaFF = () -> 0.0; + + private Debouncer debouncer = new Debouncer(0.2); + + public DriveToPose(Drivetrain drive, Supplier 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 target, + Supplier 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))); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPose.java b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java new file mode 100644 index 0000000..14e191b --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java @@ -0,0 +1,94 @@ +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 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 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 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; + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java new file mode 100644 index 0000000..63ffa73 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java @@ -0,0 +1,63 @@ +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 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 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(); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java new file mode 100644 index 0000000..37bd539 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java @@ -0,0 +1,29 @@ +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)); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java new file mode 100644 index 0000000..4fba8b1 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java @@ -0,0 +1,56 @@ +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); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java new file mode 100644 index 0000000..bf7582b --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java @@ -0,0 +1,55 @@ +// 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)); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java new file mode 100644 index 0000000..87b77a4 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java @@ -0,0 +1,51 @@ +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); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/PowerControl.java b/src/main/java/frc/robot/commands/gpm/PowerControl.java new file mode 100644 index 0000000..da3b711 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/PowerControl.java @@ -0,0 +1,76 @@ +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 + } +} diff --git a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java new file mode 100644 index 0000000..be902f7 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java @@ -0,0 +1,21 @@ +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 gamePiece, Drivetrain drive) { + // TODO: Replace DoNothing with next year's intake command + addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive))); + } +} diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java new file mode 100644 index 0000000..7479b04 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -0,0 +1,59 @@ +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 objectSupplier; + private static int ticksSinceLastObject; + private static DetectedObject cachedObject; + + public AimAtGamePiece( + Drivetrain drive, BaseDriverConfig driver, Supplier 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); + } +} diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java new file mode 100644 index 0000000..62c4755 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AimAtTag.java @@ -0,0 +1,87 @@ +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(); + } +} diff --git a/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java new file mode 100644 index 0000000..7470701 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java @@ -0,0 +1,130 @@ +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 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(); + + 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; + } +} diff --git a/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java b/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java new file mode 100644 index 0000000..572da02 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java @@ -0,0 +1,60 @@ +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, Drivetrain drive) { + super(drive, () -> getPose(detectedObject, drive)); + updateTarget = constantUpdate; + } + + @Override + public void initialize() { + cachedObject = null; + ticksSinceLastObject = 0; + super.initialize(); + } + + public static Pose2d getPose(Supplier 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); + } +} diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java new file mode 100644 index 0000000..72c2a35 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java @@ -0,0 +1,78 @@ +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 poseSupplier; + private final Drivetrain drive; + private Pose2d pose; + private double vx; + private double vy; + private Pose2d error; + + public GoToPose2(Supplier 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; + } +} diff --git a/src/main/java/frc/robot/commands/vision/LogVision.java b/src/main/java/frc/robot/commands/vision/LogVision.java new file mode 100644 index 0000000..fec9ae3 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/LogVision.java @@ -0,0 +1,38 @@ +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 objectSupplier; + + public LogVision(Supplier 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; + } +} diff --git a/src/main/java/frc/robot/commands/vision/ReturnData.java b/src/main/java/frc/robot/commands/vision/ReturnData.java new file mode 100644 index 0000000..7626443 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/ReturnData.java @@ -0,0 +1,86 @@ +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; + } +} diff --git a/src/main/java/frc/robot/commands/vision/ShutdownAllPis.java b/src/main/java/frc/robot/commands/vision/ShutdownAllPis.java new file mode 100644 index 0000000..94330f5 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/ShutdownAllPis.java @@ -0,0 +1,16 @@ +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); + } +} diff --git a/src/main/java/frc/robot/commands/vision/ShutdownOrangePi.java b/src/main/java/frc/robot/commands/vision/ShutdownOrangePi.java new file mode 100644 index 0000000..a1721a4 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/ShutdownOrangePi.java @@ -0,0 +1,119 @@ +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."); + } + } +} diff --git a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java new file mode 100644 index 0000000..8ded880 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java @@ -0,0 +1,117 @@ +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); + } +} diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java new file mode 100644 index 0000000..984f8b9 --- /dev/null +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -0,0 +1,48 @@ +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); + } + } +} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java new file mode 100644 index 0000000..066f3bb --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,92 @@ +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"; +} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java new file mode 100644 index 0000000..f45163a --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,17 @@ +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(); +} diff --git a/src/main/java/frc/robot/constants/GyroBiasConstants.java b/src/main/java/frc/robot/constants/GyroBiasConstants.java new file mode 100644 index 0000000..867c582 --- /dev/null +++ b/src/main/java/frc/robot/constants/GyroBiasConstants.java @@ -0,0 +1,25 @@ +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; +} diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java new file mode 100644 index 0000000..b9c2930 --- /dev/null +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -0,0 +1,42 @@ +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; +} diff --git a/src/main/java/frc/robot/constants/TestConstants.java b/src/main/java/frc/robot/constants/TestConstants.java new file mode 100644 index 0000000..628f30a --- /dev/null +++ b/src/main/java/frc/robot/constants/TestConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +/** Container class for test constants. */ +public class TestConstants { + public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6; +} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..b750745 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,192 @@ +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; + + //

    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. + * + *

    The options are: + * + *

    0: Disable driver assist + * + *

    1: Completely remove the component of the driver's input that is not toward the object + * + *

    2: Interpolate between the next achievable driver speed and a speed calculated using + * trapezoid profiles + * + *

    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. + * + *

    Only affects manual calculations. + * + *

    To find this, set it to 1 and measure the actual distance and the calculated distance. + * + *

    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 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 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. + * + *

    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 + * + *

    Everything is in meters and radians + * + *

    0 for all numbers is center of the robot, on the ground, looking straight toward the front + * + *

    + X: Front of Robot + * + *

    + Y: Left of Robot + * + *

    + Z: Top of Robot + * + *

    + Pitch: Down + * + *

    + Yaw: Counterclockwise + */ + public static final ArrayList> APRIL_TAG_CAMERAS = + new ArrayList>( + List.of( + new Pair( + "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( + "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( + "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( + "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 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"; +} diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java new file mode 100644 index 0000000..5b56ba6 --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -0,0 +1,285 @@ +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. + * + *

    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. + * + *

    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); + } + } +} diff --git a/src/main/java/frc/robot/constants/swerve/ModuleConstants.java b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java new file mode 100644 index 0000000..289f55f --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java @@ -0,0 +1,143 @@ +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; + } +} diff --git a/src/main/java/frc/robot/constants/swerve/ModuleType.java b/src/main/java/frc/robot/constants/swerve/ModuleType.java new file mode 100644 index 0000000..c5ad1d7 --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/ModuleType.java @@ -0,0 +1,26 @@ +package frc.robot.constants.swerve; + +/** + * Represents the type for a module on the robot. + * + *

    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(); + } +} diff --git a/src/main/java/frc/robot/controls/BaseDriverConfig.java b/src/main/java/frc/robot/controls/BaseDriverConfig.java new file mode 100644 index 0000000..7887b62 --- /dev/null +++ b/src/main/java/frc/robot/controls/BaseDriverConfig.java @@ -0,0 +1,88 @@ +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(); +} diff --git a/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java new file mode 100644 index 0000000..454d4c7 --- /dev/null +++ b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java @@ -0,0 +1,68 @@ +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; + } +} diff --git a/src/main/java/frc/robot/controls/GameControllerDriverConfig.java b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java new file mode 100644 index 0000000..b59eed3 --- /dev/null +++ b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java @@ -0,0 +1,88 @@ +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; + } +} diff --git a/src/main/java/frc/robot/controls/MadCatzDriverConfig.java b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java new file mode 100644 index 0000000..b57cf9d --- /dev/null +++ b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java @@ -0,0 +1,68 @@ +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; + } +} diff --git a/src/main/java/frc/robot/controls/Operator.java b/src/main/java/frc/robot/controls/Operator.java new file mode 100644 index 0000000..46e807f --- /dev/null +++ b/src/main/java/frc/robot/controls/Operator.java @@ -0,0 +1,49 @@ +// 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; + } +} diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java new file mode 100644 index 0000000..7b8117d --- /dev/null +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -0,0 +1,83 @@ +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; + } +} diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java new file mode 100644 index 0000000..fd48429 --- /dev/null +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -0,0 +1,153 @@ +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. + * + *

    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); + } +} diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java new file mode 100644 index 0000000..73c7725 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED/LED.java @@ -0,0 +1,176 @@ +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))); + } +} diff --git a/src/main/java/frc/robot/subsystems/PowerControl/Battery.java b/src/main/java/frc/robot/subsystems/PowerControl/Battery.java new file mode 100644 index 0000000..0efc60c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PowerControl/Battery.java @@ -0,0 +1,35 @@ +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(); + } +} diff --git a/src/main/java/frc/robot/subsystems/PowerControl/BatteryConstants.java b/src/main/java/frc/robot/subsystems/PowerControl/BatteryConstants.java new file mode 100644 index 0000000..8eafe73 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PowerControl/BatteryConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.PowerControl; + +public class BatteryConstants { + public static final double MAX_STARTING_VOLTS = 12.5; // V +} diff --git a/src/main/java/frc/robot/subsystems/PowerControl/BreakerConstants.java b/src/main/java/frc/robot/subsystems/PowerControl/BreakerConstants.java new file mode 100644 index 0000000..f4a5740 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PowerControl/BreakerConstants.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.PowerControl; + +import java.util.LinkedHashMap; +import java.util.Map; + +public class BreakerConstants { + public static final Map 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) +} diff --git a/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java new file mode 100644 index 0000000..db37648 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java @@ -0,0 +1,144 @@ +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 filters = + new ArrayList<>(); // contains currents with their alphas and thresholds + private List subsystems = new ArrayList<>(); + + public EMABreaker() { + for (Map.Entry 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java new file mode 100644 index 0000000..0bc2f0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -0,0 +1,849 @@ +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. + * + *

    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 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 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 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 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 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); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java new file mode 100644 index 0000000..3104977 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java @@ -0,0 +1,44 @@ +// 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) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java new file mode 100644 index 0000000..a768981 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java @@ -0,0 +1,83 @@ +// 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 yaw = pigeon.getYaw(); + private final StatusSignal accelrationx = pigeon.getAccelerationX(); + private final StatusSignal accelrationy = pigeon.getAccelerationY(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal 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 getYawSignal() { + return yaw; + } + + @Override + public void setYaw(Rotation2d rotation) { + pigeon.getConfigurator().setYaw(rotation.getDegrees()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java new file mode 100644 index 0000000..634fd2d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -0,0 +1,551 @@ +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 drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Timestamp inputs from Phoenix thread + protected final Queue timestampQueue; + protected final Queue drivePositionQueue; + protected final Queue 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 getDrivePositionSignal() { + return drivePosition; + } + + /** returns the turn position status signal for time-synced odometry. */ + public StatusSignal getTurnPositionSignal() { + return turnPosition; + } + + /** returns the turn absolute position status signal for time-synced odometry. */ + public StatusSignal getTurnAbsolutePositionSignal() { + return turnAbsolutePosition; + } + + public TalonFX[] getMotors() { + return new TalonFX[] {angleMotor, driveMotor}; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java new file mode 100644 index 0000000..c4790d1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java @@ -0,0 +1,50 @@ +// 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() {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java new file mode 100644 index 0000000..f6a3d6b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java @@ -0,0 +1,149 @@ +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; + } +} diff --git a/src/main/java/frc/robot/util/AngledElevatorSim.java b/src/main/java/frc/robot/util/AngledElevatorSim.java new file mode 100644 index 0000000..e7f65ae --- /dev/null +++ b/src/main/java/frc/robot/util/AngledElevatorSim.java @@ -0,0 +1,105 @@ +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 updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + // Calculate updated x-hat from Runge-Kutta. + var updatedXhat = + NumericalIntegration.rkdp( + (x, _u) -> { + Matrix 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; + } +} diff --git a/src/main/java/frc/robot/util/BuildData.java b/src/main/java/frc/robot/util/BuildData.java new file mode 100644 index 0000000..74fc017 --- /dev/null +++ b/src/main/java/frc/robot/util/BuildData.java @@ -0,0 +1,19 @@ +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(){} +} diff --git a/src/main/java/frc/robot/util/ChineseRemainderTheorem.java b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java new file mode 100644 index 0000000..b0c291b --- /dev/null +++ b/src/main/java/frc/robot/util/ChineseRemainderTheorem.java @@ -0,0 +1,48 @@ +package frc.robot.util; + +public final class ChineseRemainderTheorem { + + private ChineseRemainderTheorem() {} + + /** + * Computes x such that: x ≡ a (mod n1) x ≡ b (mod n2) + * + *

    n1 and n2 MUST be coprime. + * + *

    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); + } +} diff --git a/src/main/java/frc/robot/util/ClimbArmSim.java b/src/main/java/frc/robot/util/ClimbArmSim.java new file mode 100644 index 0000000..bfbd9da --- /dev/null +++ b/src/main/java/frc/robot/util/ClimbArmSim.java @@ -0,0 +1,183 @@ +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 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 updateX(Matrix currentXhat, Matrix 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 updatedXhat = + NumericalIntegration.rkdp( + (Matrix x, Matrix _u) -> { + Matrix 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; + } +} diff --git a/src/main/java/frc/robot/util/ConversionUtils.java b/src/main/java/frc/robot/util/ConversionUtils.java new file mode 100644 index 0000000..a89b3ae --- /dev/null +++ b/src/main/java/frc/robot/util/ConversionUtils.java @@ -0,0 +1,152 @@ +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. + * + *

    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. + * + *

    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()); + } +} diff --git a/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java new file mode 100644 index 0000000..0d1b0ae --- /dev/null +++ b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java @@ -0,0 +1,197 @@ +// 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; + } +} diff --git a/src/main/java/frc/robot/util/Elastic.java b/src/main/java/frc/robot/util/Elastic.java new file mode 100644 index 0000000..4e7d6a3 --- /dev/null +++ b/src/main/java/frc/robot/util/Elastic.java @@ -0,0 +1,390 @@ +// 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. + * + *

    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 + * + *

    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 + * + *

    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 + * + *

    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 + * + *

    This sets the display time to 0 milliseconds + * + *

    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; + } + } +} diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java new file mode 100644 index 0000000..ef74af5 --- /dev/null +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -0,0 +1,29 @@ +// 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); + } + } +} diff --git a/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java new file mode 100644 index 0000000..130bcdc --- /dev/null +++ b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java @@ -0,0 +1,73 @@ +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 velocityData = new LinkedList<>(); + private final List 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(); + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 0000000..4fb46d6 --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,163 @@ +// 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); + } +} diff --git a/src/main/java/frc/robot/util/MathUtils.java b/src/main/java/frc/robot/util/MathUtils.java new file mode 100644 index 0000000..15a74e8 --- /dev/null +++ b/src/main/java/frc/robot/util/MathUtils.java @@ -0,0 +1,156 @@ +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 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 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 arrayList) { + return arrayList.stream().mapToDouble(Double::doubleValue).toArray(); + } +} diff --git a/src/main/java/frc/robot/util/ModifiedCRT.java b/src/main/java/frc/robot/util/ModifiedCRT.java new file mode 100644 index 0000000..0f6470b --- /dev/null +++ b/src/main/java/frc/robot/util/ModifiedCRT.java @@ -0,0 +1,72 @@ +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; + } +} diff --git a/src/main/java/frc/robot/util/MotorFactory.java b/src/main/java/frc/robot/util/MotorFactory.java new file mode 100644 index 0000000..89c7a77 --- /dev/null +++ b/src/main/java/frc/robot/util/MotorFactory.java @@ -0,0 +1,196 @@ +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. + * + *

    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. + * + *

    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); + } +} diff --git a/src/main/java/frc/robot/util/PathGroupLoader.java b/src/main/java/frc/robot/util/PathGroupLoader.java new file mode 100644 index 0000000..9650dff --- /dev/null +++ b/src/main/java/frc/robot/util/PathGroupLoader.java @@ -0,0 +1,69 @@ +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> pathGroups = new HashMap<>(); + private static final HashMap 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); + } +} diff --git a/src/main/java/frc/robot/util/PhoenixOdometryThread.java b/src/main/java/frc/robot/util/PhoenixOdometryThread.java new file mode 100644 index 0000000..07d2a6e --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixOdometryThread.java @@ -0,0 +1,154 @@ +// 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. + * + *

    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 genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> 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 registerSignal(StatusSignal signal) { + Queue 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 registerSignal(DoubleSupplier signal) { + Queue 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 makeTimestampQueue() { + Queue 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(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..e931e7e --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,45 @@ +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 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); + } + } +} diff --git a/src/main/java/frc/robot/util/SwerveModulePose.java b/src/main/java/frc/robot/util/SwerveModulePose.java new file mode 100644 index 0000000..4b90f0b --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveModulePose.java @@ -0,0 +1,147 @@ +// 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; + } +} diff --git a/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java new file mode 100644 index 0000000..1853fa5 --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java @@ -0,0 +1,14 @@ +// 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) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java new file mode 100644 index 0000000..f96fdc3 --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java @@ -0,0 +1,13 @@ +// 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) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java new file mode 100644 index 0000000..1d2a3db --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java @@ -0,0 +1,497 @@ +// 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. + * + *

    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> 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); + } +} diff --git a/src/main/java/frc/robot/util/SysId.java b/src/main/java/frc/robot/util/SysId.java new file mode 100644 index 0000000..7fcb2f8 --- /dev/null +++ b/src/main/java/frc/robot/util/SysId.java @@ -0,0 +1,44 @@ +// 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 driveConsumer, + Consumer logConsumer, + Subsystem subsystem, + Config config) { + sysIdRoutine = + new SysIdRoutine(config, new Mechanism(driveConsumer, logConsumer, subsystem, name)); + } + + public SysId(String name, Consumer 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); + } +} diff --git a/src/main/java/frc/robot/util/TimeAccuracyTest.java b/src/main/java/frc/robot/util/TimeAccuracyTest.java new file mode 100644 index 0000000..88511cc --- /dev/null +++ b/src/main/java/frc/robot/util/TimeAccuracyTest.java @@ -0,0 +1,52 @@ +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; + } +} diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java new file mode 100644 index 0000000..f689438 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java @@ -0,0 +1,382 @@ +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() + ")"; + } +} diff --git a/src/main/java/frc/robot/util/Vision/DriverAssist.java b/src/main/java/frc/robot/util/Vision/DriverAssist.java new file mode 100644 index 0000000..8ebf7fe --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java @@ -0,0 +1,239 @@ +// 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 + * + *

    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)); + } +} diff --git a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java new file mode 100644 index 0000000..87cf2d4 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java @@ -0,0 +1,161 @@ +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. + * + *

    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; + } +} diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java new file mode 100644 index 0000000..7c1a444 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -0,0 +1,721 @@ +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 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> 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 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 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 getEstimatedPoses( + Pose2d referencePose, DoubleUnaryOperator yawFunction) { + ArrayList 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 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 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 getEstimatedPose(Pose2d referencePose) { + + ArrayList 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 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 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 getEstimatedPose(DoubleUnaryOperator yawFunction) { + ArrayList 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; + } + } +} diff --git a/src/main/java/frc/robot/util/Vision/VisionIO.java b/src/main/java/frc/robot/util/Vision/VisionIO.java new file mode 100644 index 0000000..e770799 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/VisionIO.java @@ -0,0 +1,81 @@ +// 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 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() {} +} diff --git a/src/main/java/lib/COTSFalconSwerveConstants.java b/src/main/java/lib/COTSFalconSwerveConstants.java new file mode 100644 index 0000000..d8ce914 --- /dev/null +++ b/src/main/java/lib/COTSFalconSwerveConstants.java @@ -0,0 +1,197 @@ +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); + } +} diff --git a/src/main/java/lib/CTREModuleState.java b/src/main/java/lib/CTREModuleState.java new file mode 100644 index 0000000..8a90f5b --- /dev/null +++ b/src/main/java/lib/CTREModuleState.java @@ -0,0 +1,62 @@ +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; + } +} diff --git a/src/main/java/lib/PolynomialRegression.java b/src/main/java/lib/PolynomialRegression.java new file mode 100644 index 0000000..d35c10c --- /dev/null +++ b/src/main/java/lib/PolynomialRegression.java @@ -0,0 +1,192 @@ +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 N + * data points (yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2. + * + *

    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 { + 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 R2. + * + * @return the coefficient of determination R2, 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 R2 + */ + 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; + } +} diff --git a/src/main/java/lib/controllers/Controller.java b/src/main/java/lib/controllers/Controller.java new file mode 100644 index 0000000..3b5913b --- /dev/null +++ b/src/main/java/lib/controllers/Controller.java @@ -0,0 +1,18 @@ +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); + } +} diff --git a/src/main/java/lib/controllers/Ex3DProController.java b/src/main/java/lib/controllers/Ex3DProController.java new file mode 100644 index 0000000..93f42be --- /dev/null +++ b/src/main/java/lib/controllers/Ex3DProController.java @@ -0,0 +1,77 @@ +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; + } +} diff --git a/src/main/java/lib/controllers/GameController.java b/src/main/java/lib/controllers/GameController.java new file mode 100644 index 0000000..ca9210b --- /dev/null +++ b/src/main/java/lib/controllers/GameController.java @@ -0,0 +1,113 @@ +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); + } +} diff --git a/src/main/java/lib/controllers/MadCatzController.java b/src/main/java/lib/controllers/MadCatzController.java new file mode 100644 index 0000000..7d14d35 --- /dev/null +++ b/src/main/java/lib/controllers/MadCatzController.java @@ -0,0 +1,90 @@ +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; + } +} diff --git a/src/main/java/lib/controllers/PS5Controller.java b/src/main/java/lib/controllers/PS5Controller.java new file mode 100644 index 0000000..1aa1e9a --- /dev/null +++ b/src/main/java/lib/controllers/PS5Controller.java @@ -0,0 +1,101 @@ +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; + } +} diff --git a/src/main/java/lib/controllers/PistolController.java b/src/main/java/lib/controllers/PistolController.java new file mode 100644 index 0000000..a16876b --- /dev/null +++ b/src/main/java/lib/controllers/PistolController.java @@ -0,0 +1,52 @@ +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; + } +} diff --git a/src/test/java/frc/robot/constants/AprilTagPoseTest.java b/src/test/java/frc/robot/constants/AprilTagPoseTest.java new file mode 100644 index 0000000..2c7d4eb --- /dev/null +++ b/src/test/java/frc/robot/constants/AprilTagPoseTest.java @@ -0,0 +1,131 @@ +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>()); + + // 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 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 redPoses = + FieldConstants.field.getTags().subList(1, 16).stream().map(tag -> tag.pose).toList(); + List 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 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); + } +} diff --git a/src/test/java/frc/robot/constants/ConstantsTest.java b/src/test/java/frc/robot/constants/ConstantsTest.java new file mode 100644 index 0000000..d1cd2fa --- /dev/null +++ b/src/test/java/frc/robot/constants/ConstantsTest.java @@ -0,0 +1,90 @@ +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); + } +} diff --git a/src/test/java/frc/robot/util/ArithTest.java b/src/test/java/frc/robot/util/ArithTest.java new file mode 100644 index 0000000..97e62a9 --- /dev/null +++ b/src/test/java/frc/robot/util/ArithTest.java @@ -0,0 +1,36 @@ +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 + * + *

    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); + } +} diff --git a/src/test/java/frc/robot/util/DetectedObjectTest.java b/src/test/java/frc/robot/util/DetectedObjectTest.java new file mode 100644 index 0000000..4f221f3 --- /dev/null +++ b/src/test/java/frc/robot/util/DetectedObjectTest.java @@ -0,0 +1,82 @@ +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); + } +} diff --git a/src/test/java/frc/robot/util/PathCheck.java b/src/test/java/frc/robot/util/PathCheck.java new file mode 100644 index 0000000..c9765df --- /dev/null +++ b/src/test/java/frc/robot/util/PathCheck.java @@ -0,0 +1,34 @@ +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. + * + *

    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(); + } +} diff --git a/src/test/java/frc/robot/util/PolynomialRegressionTest.java b/src/test/java/frc/robot/util/PolynomialRegressionTest.java new file mode 100644 index 0000000..90dd283 --- /dev/null +++ b/src/test/java/frc/robot/util/PolynomialRegressionTest.java @@ -0,0 +1,22 @@ +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); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..177ee85 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "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 diff --git a/vendordeps/CommandsV2.json b/vendordeps/CommandsV2.json new file mode 100644 index 0000000..8358b09 --- /dev/null +++ b/vendordeps/CommandsV2.json @@ -0,0 +1,46 @@ +{ + "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" + ] + } + ] +} diff --git a/vendordeps/PathplannerLibSystemCoreAlpha.json b/vendordeps/PathplannerLibSystemCoreAlpha.json new file mode 100644 index 0000000..24d3c8c --- /dev/null +++ b/vendordeps/PathplannerLibSystemCoreAlpha.json @@ -0,0 +1,37 @@ +{ + "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 diff --git a/vendordeps/Phoenix6-26.50.0-alpha-1.json b/vendordeps/Phoenix6-26.50.0-alpha-1.json new file mode 100644 index 0000000..f7db60a --- /dev/null +++ b/vendordeps/Phoenix6-26.50.0-alpha-1.json @@ -0,0 +1,449 @@ +{ + "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 diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..2861f9e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,126 @@ +{ + "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 diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..cb947f8 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "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