diff --git a/2026-Robot/.vscode/launch.json b/2026-Robot/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/2026-Robot/.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/2026-Robot/.vscode/settings.json b/2026-Robot/.vscode/settings.json new file mode 100644 index 0000000..612cdd0 --- /dev/null +++ b/2026-Robot/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] +} diff --git a/2026-Robot/.wpilib/wpilib_preferences.json b/2026-Robot/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..96ea8d5 --- /dev/null +++ b/2026-Robot/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 4499 +} \ No newline at end of file diff --git a/2026-Robot/WPILib-License.md b/2026-Robot/WPILib-License.md new file mode 100644 index 0000000..645e542 --- /dev/null +++ b/2026-Robot/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/2026-Robot/build.gradle b/2026-Robot/build.gradle new file mode 100644 index 0000000..417f7e9 --- /dev/null +++ b/2026-Robot/build.gradle @@ -0,0 +1,106 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.2.1" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// 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() + + implementation 'org.json:json:20240303' + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/2026-Robot/gradle/wrapper/gradle-wrapper.jar b/2026-Robot/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..a4b76b9 Binary files /dev/null and b/2026-Robot/gradle/wrapper/gradle-wrapper.jar differ diff --git a/2026-Robot/gradle/wrapper/gradle-wrapper.properties b/2026-Robot/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..34bd9ce --- /dev/null +++ b/2026-Robot/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/2026-Robot/gradlew b/2026-Robot/gradlew new file mode 100644 index 0000000..f5feea6 --- /dev/null +++ b/2026-Robot/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/2026-Robot/gradlew.bat b/2026-Robot/gradlew.bat new file mode 100644 index 0000000..9d21a21 --- /dev/null +++ b/2026-Robot/gradlew.bat @@ -0,0 +1,94 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/2026-Robot/settings.gradle b/2026-Robot/settings.gradle new file mode 100644 index 0000000..c493958 --- /dev/null +++ b/2026-Robot/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name = 'frcHome' + url = frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/2026-Robot/src/main/deploy/2025-reefscape-welded.json b/2026-Robot/src/main/deploy/2025-reefscape-welded.json new file mode 100644 index 0000000..eb395c0 --- /dev/null +++ b/2026-Robot/src/main/deploy/2025-reefscape-welded.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.697198, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.697198, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.560809999999998, + "y": 8.05561, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.851154, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.851154, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 5.9875419999999995, + "y": -0.0038099999999999996, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.052 + } +} diff --git a/2026-Robot/src/main/deploy/sample_path.json b/2026-Robot/src/main/deploy/sample_path.json new file mode 100644 index 0000000..2da78a2 --- /dev/null +++ b/2026-Robot/src/main/deploy/sample_path.json @@ -0,0 +1,105 @@ +{ + "pathName": "Auto", + "sampleRate": 1.0, + "pathVersion": "0.0.1", + "commands": [ + { + "index": 0, + "type": "CommandBlock", + "commands": [ + "PurePursuitFollowPath", + "Log" + ], + "arguments": { + "points": [ + { + "index": 0, + "x": 0, + "y": 0, + "angle": 0, + "time": 0.0 + }, + { + "index": 1, + "x": 2, + "y": 0, + "angle": 0, + "time": 1.0 + } + ], + "message": "Path One" + } + }, + { + "index": 1, + "type": "Switch", + "condition": "should_turn_left", + "conditionArguments": { + "value": 67 + }, + "onTrue": [ + { + "type": "CommandBlock", + "commands": [ + "FollowPath" + ], + "arguments": { + "points": [ + { + "index": 0, + "x": 2, + "y": 0, + "angle": 0, + "time": 1.0 + }, + { + "index": 1, + "x": 2, + "y": 1, + "angle": 0, + "time": 2.0 + } + ] + } + } + ], + "onFalse": [ + { + "type": "CommandBlock", + "commands": [ + "FollowPath" + ], + "condition": "should_turn_left", + "arguments": { + "points": [ + { + "index": 0, + "x": 2, + "y": 0, + "angle": 0, + "time": 1.0 + }, + { + "index": 1, + "x": 2, + "y": -1, + "angle": 0, + "time": 2.0 + } + ] + } + } + ] + }, + { + "index": 2, + "type": "CommandBlock", + "commands": [ + "Log" + ], + "arguments": { + "message": "Auto Complete" + } + } + ] +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Constants.java b/2026-Robot/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..10c7c8c --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/Constants.java @@ -0,0 +1,4316 @@ +// 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.io.File; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public final class Constants { + public static final class Autonomous { + public static final int STAGNATE_BOOST = 25; + public static final int STAGNATE_THRESHOLD = 8; // Number of cycles of stagnation before ending path + // lookahead distance is a function: + // LOOKAHEAD = AUTONOMOUS_LOOKAHEAD_DISTANCE * velocity + MIN_LOOKAHEAD_DISTANCE + // their constants + public static final double AUTONOMOUS_LOOKAHEAD_DISTANCE = 0.04; // Lookahead at 1m/s scaled by wanted + // velocity + public static final double FULL_SEND_LOOKAHEAD = 0.60; + public static final double MIN_LOOKAHEAD_DISTANCE = 0.05; // Lookahead distance at 0m/s + // Path follower will end if within this radius of the final point + public static final double AUTONOMOUS_END_ACCURACY = 0.40; + public static final double ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY = 0.05; + // When calculating the point distance, will divide x and y by this constant + public static final double AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS = 1.0; + // When calculating the point distance, will divide theta by this constant + public static final double AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS = 4 * Math.PI; + // Feed Forward Multiplier + public static final double FEED_FORWARD_MULTIPLIER = 0.8044; + public static final double ACCURATE_FOLLOWER_FEED_FORWARD_MULTIPLIER = 1; + public static String[] paths; + + static { + ArrayList autoPaths = new ArrayList<>(); + File[] dir = Filesystem.getDeployDirectory().listFiles(); + for (File file : dir) { + if (file.getName().contains(".json")) { + autoPaths.add(file.getName()); + } + } + paths = new String[autoPaths.size()]; + for (int i = 0; i < autoPaths.size(); i++) { + paths[i] = autoPaths.get(i); + } + } + + public static int getSelectedPathIndex() { + String path = OI.getSelectedPath(); + if (path.equals("None")) { + return -1; + } + for (int i = 0; i < paths.length; i++) { + if (path.equals(paths[i])) { + return i; + } + } + return -1; + } + + } + + public static void periodic() { + int index = Autonomous.getSelectedPathIndex(); + if (index == -1 || index > Constants.Autonomous.paths.length) { + Logger.recordOutput("Selected Auto", "Do Nothing"); + } else { + Logger.recordOutput("Selected Auto", Autonomous.paths[index]); + } + } + + public static void init() { + + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint1); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint2); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint3); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint4); + // // Only have these 4 now + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint5); + // // The rest are 0, 0 + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint6); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint7); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint8); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint9); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint10); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint11); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint12); + + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint1); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint2); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint3); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint4); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint5); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint6); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint7); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint8); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint9); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint10); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint11); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint12); + + // for (int i = 0; i < Constants.Vision.redSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.redSideReefTags[i][0], + // Constants.Vision.redSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.redSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.redSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // } + // for (int i = 0; i < Constants.Vision.blueSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.blueSideReefTags[i][0], + // Constants.Vision.blueSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.blueSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.blueSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // } + + // Logger.recordOutput("red side scoring", + // Constants.Physical.redCoralScoringPositions.toString()); + // Logger.recordOutput("blue side scoring", + // Constants.Physical.blueCoralScoringPositions.toString()); + System.out.println("blue algae front positions: " + + Constants.Reef.algaeBlueFrontPlacingPositions.toString()); + System.out.println("red algae front positions: " + + Constants.Reef.algaeRedFrontPlacingPositions.toString()); + System.out.println("blue algae back positions: " + + Constants.Reef.algaeBlueBackPlacingPositions.toString()); + System.out.println( + "red algae back positions: " + Constants.Reef.algaeRedBackPlacingPositions.toString()); + + System.out.println("blue positions: " + Constants.Reef.blueFrontPlacingPositions.toString()); + System.out.println("red positions: " + Constants.Reef.redFrontPlacingPositions.toString()); + System.out.println("blue back positions: " + Constants.Reef.blueBackPlacingPositions.toString()); + System.out.println("red back positions: " + Constants.Reef.redBackPlacingPositions.toString()); + + System.out.println("l4 blue positions: " + Constants.Reef.l4BlueFrontPlacingPositions.toString()); + System.out.println("l4 red positions: " + Constants.Reef.l4RedFrontPlacingPositions.toString()); + System.out.println("l4 blue back positions: " + Constants.Reef.l4BlueBackPlacingPositions.toString()); + System.out.println("l4 red back positions: " + Constants.Reef.l4RedBackPlacingPositions.toString()); + + System.out.println("l3 blue positions: " + Constants.Reef.l3BlueFrontPlacingPositions.toString()); + System.out.println("l3 red positions: " + Constants.Reef.l3RedFrontPlacingPositions.toString()); + System.out.println("l3 blue back positions: " + Constants.Reef.l3BlueBackPlacingPositions.toString()); + System.out.println("l3 red back positions: " + Constants.Reef.l3RedBackPlacingPositions.toString()); + + System.out.println("L1 Blue Corners: " + Constants.Reef.l1BlueCornerPoints.toString()); + System.out.println("L1 Red Corners: " + Constants.Reef.l1RedCornerPoints.toString()); + + System.out.println("L1 Blue Drive: " + Constants.Reef.l1BlueDrivePoints.toString()); + System.out.println("L1 Red Drive: " + Constants.Reef.l1RedDrivePoints.toString()); + + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + Logger.recordOutput("L1 Blue Corners " + i + " ", Constants.Reef.l1BlueDrivePoints.get(i)); + } + + Logger.recordOutput("feeder Positions", new Pose2d[] { Constants.Reef.RED_LEFT_FEEDER_LEFT, + Constants.Reef.RED_RIGHT_FEEDER_RIGHT, Constants.Reef.RED_RIGHT_FEEDER_LEFT, + Constants.Reef.RED_LEFT_FEEDER_RIGHT, }); + } + + public static class Reef { + public static final double PERFECT_BRANCH_OFFSET_L23 = inchesToMeters(1.625); + public static final double PERFECT_BRANCH_OFFSET_L4 = inchesToMeters(1.125); + + // positive is from face of reef towards center of reef + // negative means futher from reef + // public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.875); + // public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.25); + // public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(-0.5940); + // public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.375); + // public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.7157); + // public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.75); + + // public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.125); + // public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + // public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + + // public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.5); + // public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.875); + // public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.25); + // public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.25); + // public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + + // // // right when facing the reef side is positive + // // // negative makes robot go more to the left + // public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); // + // TODO: make red and blue + // // seperate + // public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + + public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + // right when facing the reef side is positive + // negative makes robot go more to the left + public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final Translation2d centerBlue = new Translation2d(inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final Translation2d centerRed = new Translation2d( + Constants.Physical.FIELD_LENGTH - inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final double faceToZoneLine = inchesToMeters(12); // Side of the reef to the inside of the + // reef + // zone line + + public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in + // clockwise + // order + // public static final List> + // blueBranchPositions = new + // ArrayList<>(); // Starting at the right + // // branch facing the driver + // // station in clockwise + // public static final List> + // redBranchPositions = new + // ArrayList<>(); // Starting at the right + + public static final double RED_LEFT_FEEDER_X = 16.28; + public static final double RED_LEFT_FEEDER_Y = 0.92; + public static final double RED_LEFT_FEEDER_THETA = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_X_TELEOP = 16.544; + public static final double RED_LEFT_FEEDER_Y_TELEOP = 0.890; + public static final double RED_LEFT_FEEDER_THETA_TELEOP = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_LEFT_X = 16.873; + public static final double RED_LEFT_FEEDER_LEFT_Y = 1.209; + public static final double RED_LEFT_FEEDER_LEFT_THETA = Math.toRadians(126.0); + public static final double RED_LEFT_FEEDER_RIGHT_X = 15.952; + public static final double RED_LEFT_FEEDER_RIGHT_Y = 0.564; + public static final double RED_LEFT_FEEDER_RIGHT_THETA = Math.toRadians(126.0); + + public static final Pose2d RED_LEFT_FEEDER_LEFT = new Pose2d(RED_LEFT_FEEDER_LEFT_X, + RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(RED_LEFT_FEEDER_LEFT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_LEFT = new Pose2d( + RED_LEFT_FEEDER_LEFT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(-54.0))); + public static final Pose2d RED_LEFT_FEEDER_RIGHT = new Pose2d(RED_LEFT_FEEDER_RIGHT_X, + RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(RED_LEFT_FEEDER_RIGHT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_RIGHT = new Pose2d( + RED_LEFT_FEEDER_RIGHT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER = new Pose2d(RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(RED_LEFT_FEEDER_THETA)); + public static final Pose2d RED_RIGHT_FEEDER = new Pose2d( + RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER_TELEOP = new Pose2d(RED_LEFT_FEEDER_X_TELEOP, + RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(RED_LEFT_FEEDER_THETA_TELEOP)); + public static final Pose2d RED_RIGHT_FEEDER_TELEOP = new Pose2d( + RED_LEFT_FEEDER_X_TELEOP, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(-54.0))); + + public static final double PROCESSOR_Y_OFFSET_M = inchesToMeters(50.0); + public static final double PROCESSOR_MORE_Y_OFFSET_M = inchesToMeters(25.0); + public static final double NET_X_OFFSET_M = inchesToMeters(53.0); + public static final double NET_X_OFFSET_MORE = inchesToMeters(34.0); + + public static final Translation2d processorBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedFrontPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueBackPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedBackPlacingRotation = new Rotation2d(degreesToRadians(270)); + + public static final Translation2d processorMoreBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorMoreRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedFrontPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueBackPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedBackPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Pose2d processorBlueFrontPlacingPosition = new Pose2d( + processorBlueFrontPlacingTranslation, + processorBlueFrontPlacingRotation); + public static final Pose2d processorRedFrontPlacingPosition = new Pose2d( + processorRedFrontPlacingTranslation, + processorRedFrontPlacingRotation); + public static final Pose2d processorBlueBackPlacingPosition = new Pose2d( + processorBlueBackPlacingTranslation, + processorBlueBackPlacingRotation); + public static final Pose2d processorRedBackPlacingPosition = new Pose2d( + processorRedBackPlacingTranslation, + processorRedBackPlacingRotation); + + public static final Pose2d processorMoreBlueFrontPlacingPosition = new Pose2d( + processorMoreBlueFrontPlacingTranslation, + processorMoreBlueFrontPlacingRotation); + public static final Pose2d processorMoreRedFrontPlacingPosition = new Pose2d( + processorMoreRedFrontPlacingTranslation, + processorMoreRedFrontPlacingRotation); + public static final Pose2d processorMoreBlueBackPlacingPosition = new Pose2d( + processorMoreBlueBackPlacingTranslation, + processorMoreBlueBackPlacingRotation); + public static final Pose2d processorMoreRedBackPlacingPosition = new Pose2d( + processorMoreRedBackPlacingTranslation, + processorMoreRedBackPlacingRotation); + + public static final double netBlueXM = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_M; + public static final double netRedXM = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_M; + public static final double netBlueXMore = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_MORE; + public static final double netRedXMore = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_MORE; + + public static final double netBlueFrontThetaR = degreesToRadians(0.0); + public static final double netRedFrontThetaR = degreesToRadians(180.0); + public static final double netBlueBackThetaR = degreesToRadians(180.0); + public static final double netRedBackThetaR = degreesToRadians(0.0); + + public static final List blueL1FrontPlacingPositions = new ArrayList<>(); + public static final List redL1FrontPlacingPositions = new ArrayList<>(); + public static final List blueL1BackPlacingPositions = new ArrayList<>(); + public static final List redL1BackPlacingPositions = new ArrayList<>(); + + public static final List blueL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List redL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueL1BackPlacingPositionsMore = new ArrayList<>(); + public static final List redL1BackPlacingPositionsMore = new ArrayList<>(); + + public static final List blueFrontPlacingPositions = new ArrayList<>(); + public static final List redFrontPlacingPositions = new ArrayList<>(); + public static final List blueBackPlacingPositions = new ArrayList<>(); + public static final List redBackPlacingPositions = new ArrayList<>(); + + public static final List blueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List redFrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueBackPlacingPositionsMore = new ArrayList<>(); + public static final List redBackPlacingPositionsMore = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMoreMore = new ArrayList<>(); + + public static final List l4BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l4RedFrontPlacingPositions = new ArrayList<>(); + public static final List l4BlueBackPlacingPositions = new ArrayList<>(); + public static final List l4RedBackPlacingPositions = new ArrayList<>(); + + public static final List l3BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l3RedFrontPlacingPositions = new ArrayList<>(); + public static final List l3BlueBackPlacingPositions = new ArrayList<>(); + public static final List l3RedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositions = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositions = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositions = new ArrayList<>(); + public static final List algaeRedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMore = new ArrayList<>(); + + public static final List l1BlueCornerPoints = new ArrayList<>(); + public static final List l1RedCornerPoints = new ArrayList<>(); + public static final List l1BlueDrivePoints = new ArrayList<>(); + public static final List l1RedDrivePoints = new ArrayList<>(); + + static { + calculateReefPoints(); + } + + // Angle to face red side: + // 180=AB + // 120=CD + // 60=EF + // 0=GH + // -60=IJ + // -120=KL + + public static void calculateReefPoints() { + blueL1FrontPlacingPositions.clear(); + redL1FrontPlacingPositions.clear(); + blueL1BackPlacingPositions.clear(); + redL1BackPlacingPositions.clear(); + blueL1FrontPlacingPositionsMore.clear(); + redL1FrontPlacingPositionsMore.clear(); + blueL1BackPlacingPositionsMore.clear(); + redL1BackPlacingPositionsMore.clear(); + blueFrontPlacingPositions.clear(); + redFrontPlacingPositions.clear(); + blueBackPlacingPositions.clear(); + redBackPlacingPositions.clear(); + blueFrontPlacingPositionsMore.clear(); + redFrontPlacingPositionsMore.clear(); + blueBackPlacingPositionsMore.clear(); + redBackPlacingPositionsMore.clear(); + l4BlueFrontPlacingPositions.clear(); + l4RedFrontPlacingPositions.clear(); + l4BlueBackPlacingPositions.clear(); + l4RedBackPlacingPositions.clear(); + l3BlueFrontPlacingPositions.clear(); + l3RedFrontPlacingPositions.clear(); + l3BlueBackPlacingPositions.clear(); + l3RedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositions.clear(); + algaeRedFrontPlacingPositions.clear(); + algaeBlueBackPlacingPositions.clear(); + algaeRedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositionsMore.clear(); + algaeRedFrontPlacingPositionsMore.clear(); + algaeBlueBackPlacingPositionsMore.clear(); + algaeRedBackPlacingPositionsMore.clear(); + algaeBlueFrontPlacingPositionsMoreMore.clear(); + algaeRedFrontPlacingPositionsMoreMore.clear(); + algaeBlueBackPlacingPositionsMoreMore.clear(); + algaeRedBackPlacingPositionsMoreMore.clear(); + l1BlueCornerPoints.clear(); + l1RedCornerPoints.clear(); + l1BlueDrivePoints.clear(); + l1RedDrivePoints.clear(); + centerFaces[0] = new Pose2d( + inchesToMeters(144.003), + inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = new Pose2d( + inchesToMeters(160.373), + inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = new Pose2d( + inchesToMeters(193.116), + inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = new Pose2d( + inchesToMeters(209.489), + inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = new Pose2d( + inchesToMeters(193.118), + inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = new Pose2d( + inchesToMeters(160.375), + inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + for (int face = 0; face < 6; face++) { + Pose2d l1FrontRight = new Pose2d(); + Pose2d l1FrontLeft = new Pose2d(); + Pose2d l1BackRight = new Pose2d(); + Pose2d l1BackLeft = new Pose2d(); + Pose2d l1FrontRightMore = new Pose2d(); + Pose2d l1FrontLeftMore = new Pose2d(); + Pose2d l1BackRightMore = new Pose2d(); + Pose2d l1BackLeftMore = new Pose2d(); + Pose2d l2FrontRight = new Pose2d(); + Pose2d l2FrontLeft = new Pose2d(); + Pose2d frontRightMore = new Pose2d(); + Pose2d frontLeftMore = new Pose2d(); + Pose2d backRightMore = new Pose2d(); + Pose2d backLeftMore = new Pose2d(); + Pose2d l2BackRight = new Pose2d(); + Pose2d l2BackLeft = new Pose2d(); + Pose2d l3FrontRight = new Pose2d(); + Pose2d l3FrontLeft = new Pose2d(); + Pose2d l3BackRight = new Pose2d(); + Pose2d l3BackLeft = new Pose2d(); + Pose2d l4FrontRight = new Pose2d(); + Pose2d l4FrontLeft = new Pose2d(); + Pose2d l4BackRight = new Pose2d(); + Pose2d l4BackLeft = new Pose2d(); + Pose2d algaeFront = new Pose2d(); + Pose2d algaeBack = new Pose2d(); + Pose2d algaeFrontMore = new Pose2d(); + Pose2d algaeBackMore = new Pose2d(); + Pose2d algaeFrontMoreMore = new Pose2d(); + Pose2d algaeBackMoreMore = new Pose2d(); + Pose2d l1Corner = new Pose2d(); + Pose2d l1Drive = new Pose2d(); + Pose2d poseDirection = new Pose2d(centerBlue, + Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = inchesToMeters(30.738); + double adjustY = inchesToMeters(6.469); + double adjustXL1 = inchesToMeters(30.738); + double adjustYL1 = inchesToMeters(6.469); + double adjustXMore = inchesToMeters(41.67); + double adjustYMore = inchesToMeters(6.469); + double adjustAlgaeX = inchesToMeters(45.738); + double adjustAlgaeY = inchesToMeters(0.0); + double adjustAlgaeMoreX = inchesToMeters(16.738); + double adjustAlgaeMoreY = inchesToMeters(0.0); + double adjustAlgaeMoreMoreX = inchesToMeters(56.738); + double adjustAlgaeMoreMoreY = inchesToMeters(0.0); + double adjustL1CornerX = inchesToMeters(9.0); + double adjustL1CornerY = inchesToMeters(18.0); + double adjustL1DriveX = inchesToMeters(38.0); + double adjustL1DriveY = inchesToMeters(25.0); + + l1FrontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + algaeFront = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBack = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Corner = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Drive = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + frontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + // l2FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + + // Angle to face red side: + // 180=AB + // 120=KL + // 60=IJ + // 0=GH + // -60=EF + // -120=CD + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + // l2FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + frontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + blueFrontPlacingPositions.add(l2FrontRight); + blueFrontPlacingPositions.add(l2FrontLeft); + blueBackPlacingPositions.add(l2BackRight); + blueBackPlacingPositions.add(l2BackLeft); + blueFrontPlacingPositionsMore.add(frontRightMore); + blueFrontPlacingPositionsMore.add(frontLeftMore); + blueBackPlacingPositionsMore.add(backRightMore); + blueBackPlacingPositionsMore.add(backLeftMore); + l4BlueFrontPlacingPositions.add(l4FrontRight); + l4BlueFrontPlacingPositions.add(l4FrontLeft); + l4BlueBackPlacingPositions.add(l4BackRight); + l4BlueBackPlacingPositions.add(l4BackLeft); + l3BlueFrontPlacingPositions.add(l3FrontRight); + l3BlueFrontPlacingPositions.add(l3FrontLeft); + l3BlueBackPlacingPositions.add(l3BackRight); + l3BlueBackPlacingPositions.add(l3BackLeft); + algaeBlueFrontPlacingPositions.add(algaeFront); + algaeBlueBackPlacingPositions.add(algaeBack); + algaeBlueFrontPlacingPositionsMore.add(algaeFrontMore); + algaeBlueBackPlacingPositionsMore.add(algaeBackMore); + algaeBlueFrontPlacingPositionsMoreMore.add(algaeFrontMoreMore); + algaeBlueBackPlacingPositionsMoreMore.add(algaeBackMoreMore); + l1BlueCornerPoints.add(l1Corner); + l1BlueDrivePoints.add(l1Drive); + blueL1FrontPlacingPositions.add(l1FrontLeft); + blueL1FrontPlacingPositions.add(l1FrontRight); + blueL1BackPlacingPositions.add(l1BackLeft); + blueL1BackPlacingPositions.add(l1BackRight); + blueL1FrontPlacingPositionsMore.add(l1FrontLeftMore); + blueL1FrontPlacingPositionsMore.add(l1FrontRightMore); + blueL1BackPlacingPositionsMore.add(l1BackLeftMore); + blueL1BackPlacingPositionsMore.add(l1BackRightMore); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l1BlueCornerPoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedCornerPoints.add(redPose); + } + + for (Pose2d bluePose : l1BlueDrivePoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedDrivePoints.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : l4BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l4BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedBackPlacingPositions.add(redPose); + } + } + } + + // Physical constants (e.g. field and robot dimensions) + public static final class Physical { + public static final double FIELD_WIDTH = 8.052; + public static final double FIELD_LENGTH = 17.548; + public static final double WHEEL_DIAMETER = inchesToMeters(4); + public static final double WHEEL_CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER; + public static final double WHEEL_ROTATION_PER_METER = 1 / WHEEL_CIRCUMFERENCE; + public static final double WHEEL_TO_FRAME_DISTANCE = inchesToMeters(2.5); + public static final double TOP_SPEED = feetToMeters(30.0); + public static final double MAX_ACCELERATION = feetToMeters(30.0); // TODO: actually tune the top speed + // and max acceleration. Add a max + // deceleration if needed. + + public static final double ROBOT_LENGTH = inchesToMeters(32); + public static final double ROBOT_WIDTH = inchesToMeters(26); + public static final double MODULE_OFFSET = inchesToMeters(2.625); + public static final double ROBOT_RADIUS = Math.hypot(ROBOT_LENGTH / 2 - WHEEL_TO_FRAME_DISTANCE, + ROBOT_WIDTH / 2 - WHEEL_TO_FRAME_DISTANCE); + public static double INTAKE_X_OFFSET_FRONT = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_BACK = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_BACK = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_FRONT_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_FRONT_ALGAE = inchesToMeters(3.8); + public static double INTAKE_X_OFFSET_BACK_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_BACK_ALGAE = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_FRONT_MORE = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_BACK_MORE = inchesToMeters(-0.0); + public static double L2_INTAKE_X_OFFSET_FRONT = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L2_INTAKE_X_OFFSET_BACK = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L3_INTAKE_X_OFFSET_FRONT = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L3_INTAKE_X_OFFSET_BACK = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L4_INTAKE_X_OFFSET_FRONT = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_FRONT = inchesToMeters(-1.0); + public static double L4_INTAKE_X_OFFSET_BACK = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_BACK = inchesToMeters(-1.0); + + public static final double GRAVITY_ACCEL_MS2 = 9.806; + } + + public static final class Arm { + + public static final double L4_Score = -20.0; + public static final double L4_Place = 45.0; + public static final double L3_Score = -10.0; + public static final double L3_Place = 47.0; + public static final double L2_Score = -10.0; + public static final double L2_Place = 47.0; + public static final double L1_Place = -25.0; + public static final double HANDOFF = -91.0; + public static final double DEFAULT = -90.0; + public static final double HORIZONTAL = 5.0; + public static final double VERTICAL = 95.0; + public static final double NET = 135.0; + public static final double IDLE = 0.0; + } + + public static final class Elevator { + public static final double AUTO_L1 = inchesToMeters(18.0); + public static final double AUTO_L2 = inchesToMeters(8.0); + public static final double AUTO_L3 = inchesToMeters(24.25); + public static final double AUTO_L4 = inchesToMeters(50.0); + public static final double AUTO_SCORE_L2_HIGH = inchesToMeters(14.0); + public static final double AUTO_SCORE_L2 = inchesToMeters(5.0); + public static final double AUTO_SCORE_L3 = inchesToMeters(20.0); + public static final double AUTO_SCORE_L4 = inchesToMeters(48.0); + public static final double HANDOFF_HIGH = inchesToMeters(11.0); + public static final double HANDOFF_LOW = inchesToMeters(3.0); + public static final double ALGAE_HIGH = inchesToMeters(33); + public static final double ALGAE_LOW = inchesToMeters(14.5); + public static final double PROCESSOR = inchesToMeters(0.0); + public static final double NET = inchesToMeters(55.0); + } + + // Subsystem setpoint constants + public static final class SetPoints { + public static class IntakeSetpoints { + public static final double INTAKE_ACCELERATION = 500.0; + public static final double INTAKE_CRUISE_VELOCITY = 400.0; + public static final double INTAKE_MOTION_PROFILE_SCALAR = 1.0; + public static final double INTAKE_DOWN = -18.2; // rotations + public static final double INTAKE_UP = 0; // rotations + public static final double INTAKE_ROLLER_MAX_SPEED = 1.0; // percent + public static final double INTAKE_ROLLER_HOLDING_SPEED = 0.1; // percent + public static final double INTAKE_ROLLER_TORQUE = 80.0; // amps + public static final double INTAKE_HOLDING_TORQUE = 60.0; // amps + } + + public static final double ELEVATOR_BOTTOM_POSITION_M = 0.0; + public static final double ELEVATOR_MID_POSITION_M = inchesToMeters(26.0); // L2 after placement + public static final double ELEVATOR_TOP_POSITION_M = inchesToMeters(43.0); + public static final double ELEVATOR_L1_POSITION_M = inchesToMeters(6.6); + public static final double ELEVATOR_L2_POSITION_M = inchesToMeters(15); + public static final double ELEVATOR_AUTO_L2_POSITION_M = inchesToMeters(18.5); + public static final double ELEVATOR_AUTO_L2_POSITION_SCORE_M = inchesToMeters(16); + public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(34.25); + // public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_SCORE_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_L3_POSITION_M = inchesToMeters(28); + public static final double ELEVATOR_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_ALGAE_POSITION_M = inchesToMeters(8.0); + public static final double ELEVATOR_GROUND_CORAL_POSITION_M = inchesToMeters(5.4); + public static final double ELEVATOR_GROUND_ALGAE_POSITION_M = inchesToMeters(5.0); + public static final double ELEVATOR_FEEDER_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_OVER_POSITION_M = inchesToMeters(20); + public static final double ELEVATOR_NET_POSITION_M = inchesToMeters(65); + public static final double ELEVATOR_L2_ALGAE_POSITION_M = inchesToMeters(15.7); + public static final double ELEVATOR_L3_ALGAE_POSITION_M = inchesToMeters(37.6741); + public static final double ELEVATOR_PROCESSOR_POSITION_M = inchesToMeters(6.5); + public static final double ELEVATOR_LOLLIPOP_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_PRE_HANDOFF_POSITION_M = inchesToMeters(39.0); + public static final double ELEVATOR_HANDOFF_POSITION_M = inchesToMeters(35.0); + + public enum ElevatorPosition { + kDOWN(ELEVATOR_BOTTOM_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_BOTTOM_POSITION_M)), + kMID(ELEVATOR_MID_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_MID_POSITION_M)), + kUP(ELEVATOR_TOP_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_TOP_POSITION_M)), + kL1(ELEVATOR_L1_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L1_POSITION_M)), + kL2(ELEVATOR_L2_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L2_POSITION_M)), + kAUTOL2(ELEVATOR_AUTO_L2_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_M)), + kAUTOL2SCORE(ELEVATOR_AUTO_L2_POSITION_SCORE_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_SCORE_M)), + kAUTOL3(ELEVATOR_AUTO_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L3_POSITION_M)), + kAUTOL3SCORE(ELEVATOR_AUTO_SCORE_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_SCORE_L3_POSITION_M)), + kAUTOL4(ELEVATOR_AUTO_L4_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L4_POSITION_M)), + kL3(ELEVATOR_L3_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L3_POSITION_M)), + kL4(ELEVATOR_L4_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L4_POSITION_M)), + kALGAE(ELEVATOR_ALGAE_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_ALGAE_POSITION_M)), + kFEEDER(ELEVATOR_FEEDER_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_FEEDER_POSITION_M)), + kGROUNDCORAL(ELEVATOR_GROUND_CORAL_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_CORAL_POSITION_M)), + kGROUNDALGAE(ELEVATOR_GROUND_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_ALGAE_POSITION_M)), + kNET(ELEVATOR_NET_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_NET_POSITION_M)), + kPROCESSOR(ELEVATOR_PROCESSOR_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PROCESSOR_POSITION_M)), + kL2ALGAE(ELEVATOR_L2_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L2_ALGAE_POSITION_M)), + kL3ALGAE(ELEVATOR_L3_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L3_ALGAE_POSITION_M)), + kOVER(ELEVATOR_OVER_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_OVER_POSITION_M)), + kLOLLIPOP(ELEVATOR_LOLLIPOP_POSITION_M, Ratios.elevatorMetersToRotations( + ELEVATOR_LOLLIPOP_POSITION_M)), + kHANDOFF(ELEVATOR_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_HANDOFF_POSITION_M)), + kPREHANDOFF(ELEVATOR_PRE_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PRE_HANDOFF_POSITION_M)); + + public final double meters; + public final double rotations; + + private ElevatorPosition(double meters, double rotations) { + this.meters = meters; + this.rotations = rotations; + } + } + + public static final double PIVOT_L1_POSITION_D = 65.67; + public static final double PIVOT_L23_POSITION_D = 52.5; + // public static final double PIVOT_AUTO_L23_POSITION_D = 45.0; + public static final double PIVOT_AUTO_L2_POSITION_D = 62.0; + public static final double PIVOT_AUTO_L3_POSITION_D = 62.0; + // public static final double PIVOT_AUTO_L3_POSITION_D = 30.0; + public static final double PIVOT_AUTO_L4_POSITION_D = 0.0; + public static final double PIVOT_AUTO_L4_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D = 70.0; + public static final double PIVOT_AUTO_L3_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L2_SCORE_POSITION_D = 100.0; + public static final double PIVOT_L4_POSITION_D = 60.0; + public static final double PIVOT_UPRIGHT_POSITION_D = 45.0; + public static final double PIVOT_GROUND_ALGAE_POSITION_D = 82.5; + public static final double PIVOT_GROUND_CORAL_POSITION_FRONT_D = 111.0; + public static final double PIVOT_GROUND_CORAL_POSITION_BACK_D = -111.0; + public static final double PIVOT_GROUND_CORAL_PREP_BACK_D = -90; + // public static final double PIVOT_DEFAULT_POSITION_D = 30.0; + public static final double PIVOT_DEFAULT_POSITION_D = 0.0; + public static final double PIVOT_DEFAULT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_PREP_POSITION_D = 30.0; + public static final double PIVOT_FEEDER_POSITION_D = 21.0; + public static final double PIVOT_NET_POSITION_D = 15.0; + public static final double PIVOT_PROCESSOR_POSITION_D = 76.0; + public static final double PIVOT_REEF_ALGAE_POSITION_D = 80.0; + public static final double PIVOT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_LOLLIPOP_POSITION_D = -98.0; + public static final double PIVOT_HANDOFF_POSITION_D = 145.0; + + public enum PivotPosition { + kL1(PIVOT_L1_POSITION_D, Constants.degreesToRotations(PIVOT_L1_POSITION_D)), + kL23(PIVOT_L23_POSITION_D, Constants.degreesToRotations(PIVOT_L23_POSITION_D)), + kAUTOL2(PIVOT_AUTO_L2_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L2_POSITION_D)), + kAUTOL3(PIVOT_AUTO_L3_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L3_POSITION_D)), + kAUTOL4(PIVOT_AUTO_L4_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L4_POSITION_D)), + kL4(PIVOT_L4_POSITION_D, Constants.degreesToRotations(PIVOT_L4_POSITION_D)), + kUP(PIVOT_UPRIGHT_POSITION_D, Constants.degreesToRotations(PIVOT_UPRIGHT_POSITION_D)), + kGROUNDALGAE(PIVOT_GROUND_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_GROUND_ALGAE_POSITION_D)), + kGROUNDCORALFRONT(PIVOT_GROUND_CORAL_POSITION_FRONT_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_FRONT_D)), + kGROUNDCORALBACK(PIVOT_GROUND_CORAL_POSITION_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_BACK_D)), + kGROUNDCORALPREPBACK(PIVOT_GROUND_CORAL_PREP_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_PREP_BACK_D)), + kNET(PIVOT_NET_POSITION_D, + Constants.degreesToRotations(PIVOT_NET_POSITION_D)), + kPROCESSOR(PIVOT_PROCESSOR_POSITION_D, + Constants.degreesToRotations(PIVOT_PROCESSOR_POSITION_D)), + kAUTOL2SCORE(PIVOT_AUTO_L2_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L2_SCORE_POSITION_D)), + kAUTOL3SCORE(PIVOT_AUTO_L3_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L3_SCORE_POSITION_D)), + kAUTOL4SCORE(PIVOT_AUTO_L4_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_POSITION_D)), + kAUTOL4SCORESLOW(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D)), + kDEFAULT(PIVOT_DEFAULT_POSITION_D, Constants.degreesToRotations(PIVOT_DEFAULT_POSITION_D)), + kDEFAULTCLIMB(PIVOT_DEFAULT_CLIMB_POSITION_D, + Constants.degreesToRotations(PIVOT_DEFAULT_CLIMB_POSITION_D)), + kFEEDER(PIVOT_FEEDER_POSITION_D, Constants.degreesToRotations(PIVOT_FEEDER_POSITION_D)), + kREEFALGAE(PIVOT_REEF_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_REEF_ALGAE_POSITION_D)), + kPREP(PIVOT_PREP_POSITION_D, Constants.degreesToRotations(PIVOT_PREP_POSITION_D)), + kCLIMB(PIVOT_CLIMB_POSITION_D, Constants.degreesToRotations(PIVOT_CLIMB_POSITION_D)), + kLOLLIPOP(PIVOT_LOLLIPOP_POSITION_D, Constants.degreesToRotations(PIVOT_LOLLIPOP_POSITION_D)), + kHANDOFF(PIVOT_HANDOFF_POSITION_D, Constants.degreesToRotations(PIVOT_HANDOFF_POSITION_D)); + + public final double degrees; + public final double rotations; + + private PivotPosition(double degrees, double rotations) { + this.degrees = degrees; + this.rotations = rotations; + } + } + } + + // Vision constants (e.g. camera offsets) + public static final class Vision { + // Poses of all 16 AprilTags, {x, y, z, yaw, pitch}, in meters and radians + public static final double[][] TAG_POSES = { + { inchesToMeters(657.37), inchesToMeters(25.8), inchesToMeters(58.5), + Math.toRadians(126), + Math.toRadians(0) }, + { inchesToMeters(657.37), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(234), + Math.toRadians(0) }, + { inchesToMeters(455.15), inchesToMeters(317.15), inchesToMeters(51.25), + Math.toRadians(270), + Math.toRadians(0) }, + { inchesToMeters(365.2), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(365.2), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(530.49), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) }, + { inchesToMeters(546.87), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(530.49), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(481.39), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(25.8), inchesToMeters(58.5), Math.toRadians(54), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(306), + Math.toRadians(0) }, + { inchesToMeters(325.68), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(325.68), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(235.73), inchesToMeters(-0.15), inchesToMeters(51.25), + Math.toRadians(90), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(144), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(209.49), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) } + }; + + public static final double[][] redSideReefTags = { + TAG_POSES[6 - 1], TAG_POSES[7 - 1], TAG_POSES[8 - 1], TAG_POSES[9 - 1], + TAG_POSES[10 - 1], + TAG_POSES[11 - 1] }; + + public static final double[][] blueSideReefTags = { + TAG_POSES[6 + 11 - 1], TAG_POSES[7 + 11 - 1], TAG_POSES[8 + 11 - 1], + TAG_POSES[9 + 11 - 1], + TAG_POSES[10 + 11 - 1], TAG_POSES[11 + + 11 - 1] }; + + // Poses of cameras relative to robot, {x, y, z, rx, ry, rz}, in meters and + // radians + public static final double[] FRONT_CAMERA_POSE = { Constants.inchesToMeters(1.75), + Constants.inchesToMeters(11.625), + Constants.inchesToMeters(33.5), 0, -33.5, 0 }; + + // Standard deviation adjustments + public static final double STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE = 3; + public static final double TAG_STANDARD_DEVIATION_DISTANCE = 2; // meters + public static final double TAG_STANDARD_DEVIATION_FLATNESS = 5; + // public static final double XY_STD_DEV_SCALAR = 0.1; + + // Standard deviation regressions + /** + * Calculates the standard deviation scalar based on the distance from the tag. + * + * @param dist The distance from the tag. + * @return The standard deviation scalar. + */ + public static double getTagDistStdDevScalar(double dist) { + // double a = TAG_STANDARD_DEVIATION_FLATNESS; + // double b = 1 - a * Math.pow(TAG_STANDARD_DEVIATION_DISTANCE, 2); + // Logger.recordOutput("std devs", + // -0.000277778 * Math.pow(dist, 3) + 0.00988095 * Math.pow(dist, 2) + + // 0.00444444 * dist + 0.0371429); + // return Math.max(1, a * Math.pow(dist, 2) + b); + return 0.0000520833 * Math.pow(dist, 4) + 0.000394571 * Math.pow(dist, 3) + + 0.000440341 * Math.pow(dist, 2) + + 0.0554117 * dist + 0.0298674; + } + + /** + * Calculates the standard deviation scalar based on the number of detected + * tags. + * + * @param numTags The number of detected tags. + * @return The standard deviation scalar. + */ + public static double getNumTagStdDevScalar(int numTags) { + if (numTags == 0) { + return 99999; + } else if (numTags == 1) { + return 1; + } else if (numTags == 2) { + return 0.6; + } else { + return 0.75; + } + } + + /** + * Calculates the standard deviation of the x-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the x-coordinate. + */ + public static double getTagStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.005533021491867763 * (xOffset * xOffset + yOffset * yOffset) + - 0.010807566510145635) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation of the y-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the y-coordinate. + */ + public static double getTagStdDevY(double xOffset, double yOffset) { + return Math.max(0, 0.0055 * (xOffset * xOffset + yOffset * yOffset) - 0.01941597810542626) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the x-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the x-coordinate. + */ + public static double getTriStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.004544133588821881 * (xOffset * xOffset + yOffset * yOffset) + - 0.01955724864971872) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the y-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the y-coordinate. + */ + public static double getTriStdDevY(double xOffset, double yOffset) { + return Math.max(0, + 0.002615358015002413 * (xOffset * xOffset + yOffset * yOffset) + - 0.008955462032388808) + * STANDARD_DEVIATION_SCALAR; + } + + public static double distBetweenPose(Pose3d pose1, Pose3d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static double distBetweenPose2d(Pose2d pose1, Pose2d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static final double DISTANCE_OFFSET = 7.0; + public static final double CAMERA_ANGLE_OFFSET = 0.0; + // pitch, distance + public static final double[][] CORAL_LOOKUP_TABLE = { + { -17.72 + CAMERA_ANGLE_OFFSET, 24.5 + DISTANCE_OFFSET }, + { -12.22 + CAMERA_ANGLE_OFFSET, 28.5 + DISTANCE_OFFSET }, + { -8.02 + CAMERA_ANGLE_OFFSET, 33.0 + DISTANCE_OFFSET }, + { -5.69 + CAMERA_ANGLE_OFFSET, 38.25 + DISTANCE_OFFSET }, + { -2.73 + CAMERA_ANGLE_OFFSET, 43.75 + DISTANCE_OFFSET }, + { -0.05 + CAMERA_ANGLE_OFFSET, 50.0 + DISTANCE_OFFSET }, + { 2.09 + CAMERA_ANGLE_OFFSET, 56.0 + DISTANCE_OFFSET }, + { 3.55 + CAMERA_ANGLE_OFFSET, 61.5 + DISTANCE_OFFSET }, + { 5.87 + CAMERA_ANGLE_OFFSET, 71.75 + DISTANCE_OFFSET } + }; + + /** + * Interpolates a value from a lookup table based on the given xValue. + * + * @param xIndex The index of the x-values in the lookup table. + * @param yIndex The index of the y-values in the lookup table. + * @param xValue The x-value for which to interpolate a y-value. + * @return The interpolated y-value corresponding to the given x-value. + */ + public static double getInterpolatedValue(int xIndex, int yIndex, double xValue) { + int lastIndex = CORAL_LOOKUP_TABLE.length - 1; + if (xValue < CORAL_LOOKUP_TABLE[0][xIndex]) { + // If the xValue is closer than the first setpoint + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } else if (xValue > CORAL_LOOKUP_TABLE[lastIndex][xIndex]) { + // If the xValue is farther than the last setpoint + double returnValue = CORAL_LOOKUP_TABLE[lastIndex][yIndex]; + return returnValue; + } else { + for (int i = 0; i < CORAL_LOOKUP_TABLE.length; i++) { + if (xValue > CORAL_LOOKUP_TABLE[i][xIndex] + && xValue < CORAL_LOOKUP_TABLE[i + 1][xIndex]) { + // If the xValue is in the table of setpoints + // Calculate where xValue is between setpoints + double leftDif = xValue - CORAL_LOOKUP_TABLE[i][xIndex]; + double percent = leftDif / (CORAL_LOOKUP_TABLE[i + 1][xIndex] + - CORAL_LOOKUP_TABLE[i][xIndex]); + + double value1 = CORAL_LOOKUP_TABLE[i][yIndex]; + double value2 = CORAL_LOOKUP_TABLE[i + 1][yIndex]; + + // Interpolate in-between values for value xValue and shooter rpm + double newValue = value1 + (percent * (value2 - value1)); + + double returnValue = newValue; + return returnValue; + } + } + // Should never run + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } + } + + /** + * Calculates shooter values (flywheel velocity and note velocity) based on the + * given angle. + * + * @param angle The angle of the shooter. + * @return An array containing the calculated flywheel velocity and note + * velocity. + */ + public static double getCoralDistanceFromPitch(double angle) { + return getInterpolatedValue(0, 1, angle); + // return new double[] {25, getInterpolatedValue(1, 3, angle)}; + } + } + + // Gear ratios and conversions + public static final class Ratios { + // pivot + public static final double PIVOT_GEAR_RATIO = 23 * 64 / 24; + + // drive + public static final double DRIVE_GEAR_RATIO = 6.12; + public static final double STEER_GEAR_RATIO = 21.43; + + // elevator + public static final double ELEVATOR_FIRST_STAGE = Constants.inchesToMeters(20); + public static final double ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE = 18.067; + public static final double ELEVATOR_MOTOR_ROTATIONS_PER_METER = ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE + * (1 / ELEVATOR_FIRST_STAGE); + + public static double elevatorRotationsToMeters(double rotations) { + return rotations / ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + public static double elevatorMetersToRotations(double meters) { + return meters * ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + // intake + public static final double INTAKE_PIVOT_GEAR_RATIO = 45.0; + } + + public static boolean isReady = false; + public static final ArrayList paths = new ArrayList(); + + // Can info such as IDs + public static final class CANInfo { + public static final String CANBUS_NAME = "Canivore"; + + // drive + public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 1; + public static final int FRONT_RIGHT_ANGLE_MOTOR_ID = 2; + public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 3; + public static final int FRONT_LEFT_ANGLE_MOTOR_ID = 4; + public static final int BACK_LEFT_DRIVE_MOTOR_ID = 5; + public static final int BACK_LEFT_ANGLE_MOTOR_ID = 6; + public static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; + public static final int BACK_RIGHT_ANGLE_MOTOR_ID = 8; + public static final int FRONT_RIGHT_MODULE_CANCODER_ID = 1; + public static final int FRONT_LEFT_MODULE_CANCODER_ID = 2; + public static final int BACK_LEFT_MODULE_CANCODER_ID = 3; + public static final int BACK_RIGHT_MODULE_CANCODER_ID = 4; + + // Elevator + public static final int LEFT_ELEVATOR_MOTOR_ID = 9; + public static final int RIGHT_ELEVATOR_MOTOR_ID = 10; + + // Manipulator + public static final int MANIPULATOR_MOTOR_ID = 13; + + // Intake + public static final int INTAKE_ROLLER_MOTOR_ID = 17; + public static final int INTAKE_PIVOT_MOTOR_ID = 16; + public static final int INTAKE_BEAM_BREAK_PORT = 0; + + // Straightenator + public static final int LEFT_STRAIGHTENATOR_MOTOR_ID = 12; + public static final int RIGHT_STRAIGHTENATOR_MOTOR_ID = 13; + public static final int FAR_BEAM_BREAK_SENSOR = 0; + public static final int CLOSE_BEAM_BREAK_SENSOR = 2; + + // Arm + public static final int ARM_PIVOT_MOTOR_ID = 15; + public static final int ARM_MANIPULATOR_MOTOR_ID = 14; + } + + public static final class IntakeConstants { + public static double PIVOT_LOWER_LIMIT = 0; + public static double PIVOT_UPPER_LIMIT = 10; + } + + // Misc. controller values + public static final class OperatorConstants { + public static final double RIGHT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_STICK_DEADZONE = 0.03; + public static final double RIGHT_STICK_DEADZONE = 0.05; + } + + /** + * Converts inches to meters. + * + * @param inches The length in inches to be converted. + * @return The equivalent length in meters. + */ + public static double inchesToMeters(double inches) { + return inches / 39.37; + } + + public static double metersToInches(double meters) { + return meters * 39.37; + } + + /** + * Converts feet to meters. + * + * @param inches The length in feet to be converted. + * @return The equivalent length in meters. + */ + public static double feetToMeters(double feet) { + return feet / 3.281; + } + + /** + * Calculates the Euclidean distance between two points in a 2D plane. + * + * @param x1 The x-coordinate of the first point. + * @param y1 The y-coordinate of the first point. + * @param x2 The x-coordinate of the second point. + * @param y2 The y-coordinate of the second point. + * @return The Euclidean distance between the two points. + */ + public static double getDistance(double x1, double y1, double x2, double y2) { + return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); + } + + public static double getAngleToPoint(double x1, double y1, double x2, double y2) { + double deltaX = x2 - x1; + double deltaY = y2 - y1; + + double angleInRadians = Math.atan2(deltaY, deltaX); + + double angleInDegrees = Math.toDegrees(angleInRadians); + + double standardizeAngleDegrees = standardizeAngleDegrees(angleInDegrees); + + return 180 + standardizeAngleDegrees; + } + + /** + * Converts a quantity in rotations to radians. + * + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in radians. + */ + public static double rotationsToRadians(double rotations) { + return rotations * 2 * Math.PI; + } + + /** + * Converts a quantity in degrees to rotations. + * + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in rotations. + */ + public static double degreesToRotations(double degrees) { + return degrees / 360; + } + + /** + * Converts a quantity in rotations to degrees. + * + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in degrees. + */ + public static double rotationsToDegrees(double rotations) { + return rotations * 360; + } + + /** + * Converts a quantity in degrees to radians. + * + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in radians. + */ + public static double degreesToRadians(double degrees) { + return degrees * Math.PI / 180; + } + + /** + * Standardizes an angle to be within the range [0, 360) degrees. + * + * @param angleDegrees The input angle in degrees. + * @return The standardized angle within the range [0, 360) degrees. + */ + public static double standardizeAngleDegrees(double angleDegrees) { + return ((angleDegrees % 360) + 360) % 360; + } + + /** + * Calculates the x-component of a unit vector given an angle in radians. + * + * @param angle The angle in radians. + * @return The x-component of the unit vector. + */ + public static double angleToUnitVectorI(double angle) { + return (Math.cos(angle)); + } + + /** + * Calculates the y-component of a unit vector given an angle in radians. + * + * @param angle The angle in radians. + * @return The y-component of the unit vector. + */ + public static double angleToUnitVectorJ(double angle) { + return (Math.sin(angle)); + } + + /** + * Converts revolutions per minute (RPM) to revolutions per second (RPS). + * + * @param RPM The value in revolutions per minute (RPM) to be converted. + * @return The equivalent value in revolutions per second (RPS). + */ + public static double RPMToRPS(double RPM) { + return RPM / 60; + } + + /** + * Converts revolutions per second (RPS) to revolutions per minute (RPM). + * + * @param RPM The value in revolutions per second (RPS) to be converted. + * @return The equivalent value in revolutions per minute (RPM). + */ + public static double RPSToRPM(double RPS) { + return RPS * 60; + } + + /** + * Standardizes an angle to be within the range [otherAngle - pi, otherAngle + + * pi) radians. + * + * @param angle The input angle in radians. + * @param otherAngle The reference angle in radians. + * @return The standardized angle within the range [otherAngle - pi, otherAngle + * + + * pi) radians. + */ + public static double standardizeAngleToOther(double angle, double otherAngle) { + double delta = angle - otherAngle; + delta = Math.IEEEremainder(delta, 2 * Math.PI); // gives value in [-π, π] + return otherAngle + delta; + } + + /** + * Standardizes an angle to be within the range [otherAngle - 180, otherAngle + + * 180) degrees. + * + * @param angle The input angle in degrees. + * @param otherAngle The reference angle in degrees. + * @return The standardized angle within the range [otherAngle - 180, otherAngle + * + + * 180) degrees. + */ + public static double standardizeAngleToOtherDegrees(double angle, double otherAngle) { + return Math.toDegrees(standardizeAngleToOther(degreesToRadians(angle), degreesToRadians(otherAngle))); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Main.java b/2026-Robot/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..8776e5d --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/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 frc.robot; + +import edu.wpi.first.wpilibj.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(Robot::new); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/OI.java b/2026-Robot/src/main/java/frc/robot/OI.java new file mode 100644 index 0000000..80f2a5a --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/OI.java @@ -0,0 +1,284 @@ +// Copyrights (c) 2018-2019 FIRST, 2020 Highlanders FRC. All Rights Reserved. +//hi om + +package frc.robot; + +import java.io.File; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.tools.TriggerButton; + +public class OI { + public static SendableChooser fieldSide = new SendableChooser(); + public static SendableChooser leftRight = new SendableChooser(); + public static SendableChooser auto = new SendableChooser(); + public static XboxController driverController = new XboxController(0); + public static XboxController operatorController = new XboxController(1); + + public static BooleanSupplier driveRTSupplier = () -> getDriverRTPercent() > Constants.OperatorConstants.RIGHT_TRIGGER_DEADZONE; + public static BooleanSupplier driverLTSupplier = () -> getDriverLTPercent() > Constants.OperatorConstants.LEFT_TRIGGER_DEADZONE; + + public static BooleanSupplier povUp = () -> getPOV() == 0; + public static BooleanSupplier povRight = () -> getPOV() == 90; + public static BooleanSupplier povDown = () -> getPOV() == 180; + public static BooleanSupplier povLeft = () -> getPOV() == 270; + + public static TriggerButton driverPOVUp = new TriggerButton(povUp); + public static TriggerButton driverPOVRight = new TriggerButton(povRight); + public static TriggerButton driverPOVDown = new TriggerButton(povDown); + public static TriggerButton driverPOVLeft = new TriggerButton(povLeft); + + public static BooleanSupplier operatorPovUp = () -> getOperatorPOV() == 0; + public static BooleanSupplier operatorPovRight = () -> getOperatorPOV() == 90; + public static BooleanSupplier operatorPovDown = () -> getOperatorPOV() == 180; + public static BooleanSupplier operatorPovLeft = () -> getOperatorPOV() == 270; + + public static TriggerButton operatorPOVUp = new TriggerButton(operatorPovUp); + public static TriggerButton operatorPOVRight = new TriggerButton(operatorPovRight); + public static TriggerButton operatorPOVDown = new TriggerButton(operatorPovDown); + public static TriggerButton operatorPOVLeft = new TriggerButton(operatorPovLeft); + + public static TriggerButton driverRT = new TriggerButton(driveRTSupplier); + public static TriggerButton driverLT = new TriggerButton(driverLTSupplier); + + public static JoystickButton driverA = new JoystickButton(driverController, 1); + public static JoystickButton driverB = new JoystickButton(driverController, 2); + + public static JoystickButton driverY = new JoystickButton(driverController, 4); + public static JoystickButton driverX = new JoystickButton(driverController, 3); + + public static JoystickButton driverRB = new JoystickButton(driverController, 6); + public static JoystickButton driverLB = new JoystickButton(driverController, 5); + + public static JoystickButton driverLJ = new JoystickButton(driverController, 9); + public static JoystickButton driverRJ = new JoystickButton(driverController, 10); + + public static JoystickButton operatorX = new JoystickButton(operatorController, 3); + public static JoystickButton operatorB = new JoystickButton(operatorController, 2); + + public static JoystickButton operatorY = new JoystickButton(operatorController, 4); + public static JoystickButton operatorA = new JoystickButton(operatorController, 1); + + public static BooleanSupplier operatorRTSupplier = () -> getOperatorRTPercent() > Constants.OperatorConstants.RIGHT_TRIGGER_DEADZONE; + public static BooleanSupplier operatorLTSupplier = () -> getOperatorLTPercent() > Constants.OperatorConstants.LEFT_TRIGGER_DEADZONE; + + public static TriggerButton operatorRT = new TriggerButton(operatorRTSupplier); + public static TriggerButton operatorLT = new TriggerButton(operatorLTSupplier); + + public static JoystickButton operatorRB = new JoystickButton(operatorController, 6); + public static JoystickButton operatorLB = new JoystickButton(operatorController, 5); + + public static JoystickButton operatorLJ = new JoystickButton(operatorController, 9); + public static JoystickButton operatorRJ = new JoystickButton(operatorController, 10); + + public static JoystickButton driverViewButton = new JoystickButton(driverController, 7); + + public static JoystickButton operatorViewButton = new JoystickButton(operatorController, 7); + public static JoystickButton driverMenuButton = new JoystickButton(driverController, 8); + + public static JoystickButton operatorMenuButton = new JoystickButton(operatorController, 8); + + public static Joystick autoChooser = new Joystick(2); + + public static JoystickButton autoChooserIsBlue = new JoystickButton(autoChooser, 8); + + static { + fieldSide.addOption("red", "red"); + fieldSide.addOption("blue", "blue"); + fieldSide.setDefaultOption("blue", "blue"); + SmartDashboard.putData(fieldSide); + leftRight.addOption("processor", "processor"); + leftRight.addOption("net", "net"); + leftRight.setDefaultOption("net", "net"); + SmartDashboard.putData(leftRight); + for (String path : Constants.Autonomous.paths) { + auto.addOption(path, path); + } + auto.setDefaultOption("None", "None"); + SmartDashboard.putData(auto); + } + + public static void printAutoChooserInputs() { + java.util.logging.Logger.getGlobal().info("Driver Controller Connected: " + driverController.isConnected()); + java.util.logging.Logger.getGlobal().info("Operator Controller Connected: " + operatorController.isConnected()); + java.util.logging.Logger.getGlobal().info("Auto Chooser Connected: " + autoChooser.isConnected()); + java.util.logging.Logger.getGlobal().info("Auto Chooser Num Buttons: " + autoChooser.getButtonCount()); + java.util.logging.Logger.getGlobal().info("Is Blue: " + autoChooserIsBlue.getAsBoolean()); + for (int i = 1; i <= 16; i++) { + java.util.logging.Logger.getGlobal().info("Auto Chooser Button " + i + " : " + autoChooser.getRawButton(i)); + } + } + + public static String getSelectedPath() { + return auto.getSelected(); + } + + public static double getDriverLeftX() { + double leftX = -driverController.getLeftX(), leftY = driverController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftX = 0; + } + return leftX; + } + + public static double getDriverLeftY() { + double leftX = -driverController.getLeftX(), leftY = driverController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftY = 0; + } + return leftY; + } + + public static double getDriverRightX() { + double rightX = driverController.getRightX(), rightY = driverController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { + rightX = 0; + } + return rightX; + } + + public static double getDriverRightY() { + double rightX = driverController.getRightX(), rightY = driverController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { + rightY = 0; + } + return rightY; + } + + public static double getOperatorLeftX() { + double leftX = -operatorController.getLeftX(), leftY = operatorController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftX = 0; + } + return leftX; + } + + public static double getOperatorLeftY() { + double leftX = -operatorController.getLeftX(), leftY = operatorController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftY = 0; + } + return leftY; + } + + public static double getOperatorRightX() { + double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { + rightX = 0; + } + return rightX; + } + + public static double getOperatorRightY() { + double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { + rightY = 0; + } + return rightY; + } + + public static double getDriverRTPercent() { + return driverController.getRightTriggerAxis(); + } + + /** + * This is for using the backup controller, the LT is 1.0 when it should be at + * zero + * Set the trigger depth on the controller to T2 (middle) for LT when using this + * code + */ + public static double getDriverLTPercent() { + // extra deadzone logic for bad controller + // double raw = driverController.getLeftTriggerAxis(); + // if (raw > 0.9) { + // raw = 0.0; + // } + // double refined = raw * (4.0 / 3.0); + // return refined; + return driverController.getLeftTriggerAxis(); + } + + public static boolean getDriverA() { + return driverController.getAButton(); + } + + public static boolean getDriverRB() { + return driverController.getRightBumperButton(); + } + + public static boolean getDriverLB() { + return driverController.getLeftBumperButton(); + } + + public static double getOperatorRTPercent() { + return operatorController.getRightTriggerAxis(); + } + + public static double getOperatorLTPercent() { + return operatorController.getLeftTriggerAxis(); + } + + public static boolean getOperatorLB() { + return operatorController.getLeftBumperButton(); + } + + public static int getPOV() { + return driverController.getPOV(); + } + + public static int getOperatorPOV() { + return operatorController.getPOV(); + } + + public static boolean getPOVUp() { + if (getPOV() == 0) { + return true; + } else { + return false; + } + } + + public static boolean isRecalculateMode() { + return autoChooser.getRawButton(7); + } + + public static boolean autoChooserConnected() { + return autoChooser.isConnected(); + } + + public static boolean isBlueSide() { + return fieldSide.getSelected().equals("blue"); + } + + public static boolean is4PieceFarBottom231Auto() { + return autoChooser.getRawButton(2); + } + + public static boolean is5PieceAuto() { + return autoChooser.getRawButton(1); + } + + public static boolean is3PieceFarBottomAuto() { + return autoChooser.getRawButton(3); + } + + public static boolean is1PieceAuto() { + return autoChooser.getRawButton(5); + } + + public static boolean is4PieceAmpSideAuto() { + return autoChooser.getRawButton(4); + } + + public static boolean getDriverRightJoystickPressed() { + return driverController.getRightStickButton(); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Robot.java b/2026-Robot/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..12fef72 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/Robot.java @@ -0,0 +1,98 @@ +package frc.robot; + +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.littletonrobotics.junction.LogFileUtil; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.AutoRunner; +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; +import frc.robot.subsystems.Drive.DriveState; + +import java.io.IOException; + +public class Robot extends LoggedRobot { + + private RobotContainer m_robotContainer; + private Command m_autonomousCommand; + private Drive drive; + private Peripherals peripherals; + + @Override + public void robotInit() { + // Start AdvantageScope logging + Logger.recordMetadata("Robot", "Robot"); + if (isReal()) { + Logger.addDataReceiver(new NT4Publisher()); + } else { + setUseTiming(false); + String replayLog = LogFileUtil.findReplayLog(); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(replayLog, "_sim"))); + + } + Logger.start(); + + m_robotContainer = new RobotContainer(); + drive = m_robotContainer.drive; + peripherals = m_robotContainer.peripherals; + drive.init("blue"); // your team color or alliance color + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void autonomousInit() { + CommandScheduler.getInstance().schedule(new AutoRunner("sample_path", drive, peripherals)); + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + @Override + public void teleopPeriodic() { + drive.setWantedState(DriveState.DEFAULT); + if (OI.driverMenuButton.getAsBoolean()) { + peripherals.zeroPigeon(); + } + } + + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() { + } + + @Override + public void simulationInit() { + } + + @Override + public void simulationPeriodic() { + } +} diff --git a/2026-Robot/src/main/java/frc/robot/RobotContainer.java b/2026-Robot/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..3c18b15 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,50 @@ +package frc.robot; + +import frc.robot.commands.AutoRunner; +import frc.robot.commands.ZeroPigeon; +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; + +import java.io.File; +import java.io.FileReader; +import java.util.HashMap; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import org.json.JSONObject; +import org.json.JSONTokener; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class RobotContainer { + + public final Peripherals peripherals = new Peripherals(); + public final Drive drive = new Drive(peripherals); + + private JSONObject[] autoJSONs; + + public RobotContainer() { + } + + private void loadAutoJSONs() { + autoJSONs = new JSONObject[Constants.paths.size()]; + for (int i = 0; i < Constants.paths.size(); i++) { + try { + File file = new File(Filesystem.getDeployDirectory(), Constants.paths.get(i)); + FileReader reader = new FileReader(file); + autoJSONs[i] = new JSONObject(new JSONTokener(reader)); + } catch (Exception e) { + System.out.println("ERROR LOADING PATH " + Constants.paths.get(i) + ": " + e); + autoJSONs[i] = null; + } + } + } + + public Command getAutonomousCommand() { + if (Constants.paths.isEmpty() || autoJSONs[0] == null) + return null; + return new AutoRunner(Constants.paths.get(0), drive, peripherals); + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java b/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java new file mode 100644 index 0000000..7fa3a76 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java @@ -0,0 +1,316 @@ +package frc.robot.commands; + +import java.io.File; +import java.io.FileReader; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Set; +import java.util.function.Function; + +import org.json.JSONArray; +import org.json.JSONObject; +import org.json.JSONTokener; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; + +import frc.robot.commands.conditionals.Conditional; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class AutoRunner extends Command { + private final Drive drive; + private final Peripherals peripherals; + private final String pathName; + + private JSONArray rootCommands = new JSONArray(); + private final HashMap> conditionalMap = new HashMap<>(); + + private final List activeCommands = new ArrayList<>(); + private final List stack = new ArrayList<>(); + private final Set triggerFired = new HashSet<>(); + + private boolean odometryInitialized = false; + + private static class StackEntry { + JSONArray array; + int index; + final Conditional switchConditional; + final JSONObject switchObject; + + StackEntry(JSONArray a, int i, Conditional c, JSONObject o) { + array = a; + index = i; + switchConditional = c; + switchObject = o; + } + } + + public AutoRunner(String pathName, Drive drive, Peripherals peripherals) { + this.drive = drive; + this.peripherals = peripherals; + this.pathName = pathName; + initConditionals(); + loadPathFile(); + } + + private void initConditionals() { + conditionalMap.put("should_turn_left", args -> new frc.robot.commands.conditionals.LeftPath()); + conditionalMap.put("start_main_path", args -> new frc.robot.commands.conditionals.StartMainPath()); + conditionalMap.put("start_left_path", args -> new frc.robot.commands.conditionals.LeftPath()); + conditionalMap.put("start_right_path", args -> new frc.robot.commands.conditionals.StartRightPath()); + } + + private void loadPathFile() { + try { + FileReader r = new FileReader(new File(Filesystem.getDeployDirectory(), pathName + ".json")); + JSONObject json = new JSONObject(new JSONTokener(r)); + rootCommands = json.getJSONArray("commands"); + } catch (Exception e) { + rootCommands = new JSONArray(); + } + } + + @Override + public void initialize() { + activeCommands.clear(); + stack.clear(); + triggerFired.clear(); + odometryInitialized = false; + stack.add(new StackEntry(rootCommands, 0, null, null)); + scheduleNextIfIdle(); + } + + private Command buildSingleCommand(String name, JSONObject args) { + if (args == null) + args = new JSONObject(); + switch (name) { + case "FollowPath": + return new FollowPath(args, drive); + case "PurePursuitFollowPath": + return new PurePursuitFollowPath(args, drive); + case "Zero": + return new ZeroPigeon(peripherals); + case "Log": + return new InstantCommand(); + default: + return new InstantCommand(); + } + } + + private void tryInitializeOdometry(JSONObject args) { + if (odometryInitialized) + return; + if (args == null) + return; + JSONArray pts = args.optJSONArray("points"); + if (pts == null || pts.length() == 0) + return; + JSONObject p = pts.getJSONObject(0); + double x = p.optDouble("x", 0); + double y = p.optDouble("y", 0); + double angle = p.optDouble("angle", 0); + drive.setOdometry(new Pose2d(x, y, Rotation2d.fromDegrees(angle))); + Logger.recordOutput("AutoRunner/OdometryInitX", x); + Logger.recordOutput("AutoRunner/OdometryInitY", y); + Logger.recordOutput("AutoRunner/OdometryInitAngle", angle); + odometryInitialized = true; + } + + private Command buildCommandBlock(JSONObject block) { + JSONArray names = block.optJSONArray("commands"); + JSONObject args = block.optJSONObject("arguments"); + if (names == null || names.length() == 0) + return null; + List list = new ArrayList<>(); + for (int i = 0; i < names.length(); i++) { + String n = names.getString(i); + if (n.equals("FollowPath") || n.equals("PurePursuitFollowPath")) + tryInitializeOdometry(args); + Command c = buildSingleCommand(n, args); + if (c != null) + list.add(c); + } + if (list.isEmpty()) + return null; + Command[] arr = list.toArray(new Command[0]); + if (arr.length == 1) + return arr[0]; + return new ParallelCommandGroup(arr); + } + + private void scheduleCommandAndTrack(Command c) { + if (c == null) + return; + CommandScheduler.getInstance().schedule(c); + activeCommands.add(c); + } + + private Conditional buildConditionalInstance(String condName, Object condArgs) { + Function factory = conditionalMap.get(condName); + if (factory == null) + return null; + Conditional instance; + try { + instance = factory.apply(condArgs); + } catch (Exception e) { + instance = factory.apply(null); + } + if (instance != null && condArgs instanceof JSONObject) { + try { + instance.setArguments((JSONObject) condArgs); + } catch (Exception ignored) { + } + } + return instance; + } + + private boolean triggerConditionTrueForNode(JSONObject node) { + JSONObject trigger = node.optJSONObject("trigger"); + if (trigger == null) + return true; + if (!trigger.has("condition")) + return true; + String condName = trigger.optString("condition", ""); + if (condName.isEmpty()) + return true; + Object condArgs = trigger.has("conditionArguments") ? trigger.get("conditionArguments") : null; + Conditional c = buildConditionalInstance(condName, condArgs); + if (c == null) + return false; + return c.evaluate(); + } + + private void runTriggerCommandsIfAny(JSONObject node, String pathId) { + if (triggerFired.contains(pathId)) + return; + JSONObject trigger = node.optJSONObject("trigger"); + if (trigger == null) + return; + JSONArray cmds = trigger.optJSONArray("commands"); + if (cmds == null) + return; + for (int i = 0; i < cmds.length(); i++) { + String name = cmds.getString(i); + Command c = buildSingleCommand(name, null); + scheduleCommandAndTrack(c); + } + triggerFired.add(pathId); + } + + private String currentPathId() { + StringBuilder sb = new StringBuilder(); + for (StackEntry e : stack) + sb.append("/").append(e.index); + return sb.toString(); + } + + private void scheduleNextIfIdle() { + if (!activeCommands.isEmpty()) + return; + while (!stack.isEmpty()) { + StackEntry top = stack.get(stack.size() - 1); + if (top.index >= top.array.length()) { + stack.remove(stack.size() - 1); + continue; + } + JSONObject node = top.array.getJSONObject(top.index); + String type = node.optString("type", ""); + String pathId = currentPathId(); + if (type.equals("CommandBlock")) { + if (!triggerConditionTrueForNode(node)) + return; + runTriggerCommandsIfAny(node, pathId); + Command block = buildCommandBlock(node); + if (block != null) + scheduleCommandAndTrack(block); + top.index++; + return; + } + if (type.equals("Switch")) { + String condName = node.optString("condition", ""); + if (condName.isEmpty()) { + top.index++; + continue; + } + Object condArgs = node.has("conditionArguments") ? node.get("conditionArguments") : null; + Conditional condInstance = buildConditionalInstance(condName, condArgs); + top.index++; + if (condInstance == null) + continue; + boolean result = condInstance.evaluate(); + JSONArray branch = result ? node.optJSONArray("onTrue") : node.optJSONArray("onFalse"); + if (branch == null) + continue; + stack.add(new StackEntry(branch, 0, condInstance, node)); + return; + } + top.index++; + } + } + + private void handleActiveSwitchFlipIfNeeded() { + if (stack.isEmpty()) + return; + StackEntry top = stack.get(stack.size() - 1); + if (top.switchConditional == null) + return; + boolean now = top.switchConditional.evaluate(); + JSONObject switchObj = top.switchObject; + boolean currentlyTrue = top.array == switchObj.optJSONArray("onTrue"); + if (now != currentlyTrue) { + for (Command c : new ArrayList<>(activeCommands)) + CommandScheduler.getInstance().cancel(c); + activeCommands.clear(); + JSONArray newBranch = now ? switchObj.optJSONArray("onTrue") : switchObj.optJSONArray("onFalse"); + top.array = newBranch != null ? newBranch : new JSONArray(); + top.index = 0; + } + } + + private void cleanupFinishedActiveCommands() { + Iterator it = activeCommands.iterator(); + while (it.hasNext()) { + Command c = it.next(); + if (!CommandScheduler.getInstance().isScheduled(c)) + it.remove(); + } + } + + @Override + public void execute() { + cleanupFinishedActiveCommands(); + if (!activeCommands.isEmpty()) { + handleActiveSwitchFlipIfNeeded(); + return; + } + handleActiveSwitchFlipIfNeeded(); + scheduleNextIfIdle(); + } + + @Override + public void end(boolean interrupted) { + for (Command c : new ArrayList<>(activeCommands)) + CommandScheduler.getInstance().cancel(c); + activeCommands.clear(); + stack.clear(); + triggerFired.clear(); + drive.stop(); + } + + @Override + public boolean isFinished() { + return stack.isEmpty() && activeCommands.isEmpty(); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/Autos.java b/2026-Robot/src/main/java/frc/robot/commands/Autos.java new file mode 100644 index 0000000..b32acdb --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Autos.java @@ -0,0 +1,16 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public final class Autos { + /** Example static factory for an autonomous command. */ + + private Autos() { + throw new UnsupportedOperationException("This is a utility class!"); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java b/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java new file mode 100644 index 0000000..ef29a32 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java @@ -0,0 +1,29 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; + +public class CommandBlock extends Command { + public CommandBlock() { + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java new file mode 100644 index 0000000..c782f62 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java @@ -0,0 +1,99 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Drive; +import frc.robot.tools.PathLoader; +import frc.robot.tools.PosePoint; +import frc.robot.tools.math.Vector; + +import java.util.ArrayList; +import java.util.List; +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; + +public class FollowPath extends Command { + private final Drive drive; + private final List points = new ArrayList<>(); + private final Timer timer = new Timer(); + private int currentIndex = 0; + private static final double POSITION_TOLERANCE = 0.02; + private static final double ANGLE_TOLERANCE = 0.05; + private static final double MAX_SPEED = 6.7; // meters/sec + private static final double MAX_ANGULAR = 2.0; // rad/sec + + public FollowPath(JSONObject arguments, Drive driveSubsystem) { + this.drive = driveSubsystem; + if (arguments != null && arguments.has("points")) { + JSONArray pointsArray = arguments.getJSONArray("points"); + for (int i = 0; i < pointsArray.length(); i++) { + JSONObject p = pointsArray.getJSONObject(i); + points.add(new PosePoint( + p.getDouble("x"), + p.getDouble("y"), + p.getDouble("angle"), + p.getDouble("time"))); + } + } + } + + @Override + public void initialize() { + currentIndex = 0; + timer.reset(); + timer.start(); + } + + private double wrapAngle(double angle) { + while (angle > Math.PI) + angle -= 2 * Math.PI; + while (angle < -Math.PI) + angle += 2 * Math.PI; + return angle; + } + + @Override + public void execute() { + if (currentIndex >= points.size()) + return; + + PosePoint target = points.get(currentIndex); + Pose2d pose = drive.getMT2Odometry(); + + double dx = target.x - pose.getX(); + double dy = target.y - pose.getY(); + double distance = Math.hypot(dx, dy); + double dtheta = wrapAngle(target.theta - pose.getRotation().getRadians()); + + currentIndex = (int) (timer.getTimestamp() / 0.5); + + double vx = (distance > 0) ? (dx / distance) * Math.min(distance, MAX_SPEED) : 0; + double vy = (distance > 0) ? (dy / distance) * Math.min(distance, MAX_SPEED) : 0; + double omega = Math.signum(dtheta) * Math.min(Math.abs(dtheta), MAX_ANGULAR); + + double cos = Math.cos(-pose.getRotation().getRadians()); + double sin = Math.sin(-pose.getRotation().getRadians()); + double robotX = vx * cos - vy * sin; + double robotY = vx * sin + vy * cos; + + drive.autoDrive(new Vector(robotX, -robotY), -omega); + + Logger.recordOutput("FollowPath/X", target.x); + Logger.recordOutput("FollowPath/Y", target.y); + Logger.recordOutput("FollowPath/Theta", target.theta); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + drive.stop(); + } + + @Override + public boolean isFinished() { + return currentIndex >= points.size(); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/Log.java b/2026-Robot/src/main/java/frc/robot/commands/Log.java new file mode 100644 index 0000000..d43418e --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Log.java @@ -0,0 +1,43 @@ +// 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; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class Log extends Command { + /** Creates a new Logger. */ + public Log(JSONObject json) { + String string = ""; + if (json.has("message")) { + string = json.getString("message"); + } + System.out.println(string); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java new file mode 100644 index 0000000..1486727 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java @@ -0,0 +1,244 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Drive; +import frc.robot.tools.PathLoader; +import frc.robot.tools.PosePoint; +import frc.robot.tools.math.Vector; +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; +import java.util.List; + +public class PurePursuitFollowPath extends Command { + private final Drive drive; + private final JSONArray rawPoints; + private final JSONArray pointsWithVels; + private final List points = new ArrayList<>(); + private int currentIndex = 0; + private boolean odometrySet = false; + + private final double kP_x = 3.0; + private final double kP_y = 3.0; + private final double kP_theta = 2.7; + + private static final double POSITION_TOLERANCE = 0.02; + private static final double ANGLE_TOLERANCE = 0.05; + + public PurePursuitFollowPath(JSONObject arguments, Drive driveSubsystem) { + this.drive = driveSubsystem; + if (arguments != null && arguments.has("points")) + rawPoints = arguments.getJSONArray("points"); + else + rawPoints = new JSONArray(); + + pointsWithVels = buildPointsWithVelocities(rawPoints); + + for (int i = 0; i < rawPoints.length(); i++) { + JSONObject p = rawPoints.getJSONObject(i); + points.add(new PosePoint( + p.optDouble("x", 0.0), + p.optDouble("y", 0.0), + p.optDouble("angle", 0.0), + p.optDouble("time", 0.0))); + } + } + + private JSONArray buildPointsWithVelocities(JSONArray raw) { + JSONArray out = new JSONArray(); + int n = raw.length(); + if (n == 0) + return out; + + double[] xs = new double[n], ys = new double[n], th = new double[n], t = new double[n]; + for (int i = 0; i < n; i++) { + JSONObject p = raw.getJSONObject(i); + xs[i] = p.optDouble("x", 0.0); + ys[i] = p.optDouble("y", 0.0); + th[i] = p.optDouble("angle", 0.0); + double tt = p.has("time") ? p.optDouble("time", Double.NaN) : Double.NaN; + t[i] = Double.isNaN(tt) ? i : tt; + } + + double[] vx = new double[n], vy = new double[n], vth = new double[n]; + if (n == 1) { + vx[0] = vy[0] = vth[0] = 0.0; + } else { + for (int i = 0; i < n; i++) { + if (i == 0) { + double dt = t[1] - t[0]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[0] = (xs[1] - xs[0]) / dt; + vy[0] = (ys[1] - ys[0]) / dt; + vth[0] = wrap(th[1] - th[0]) / dt; + } else if (i == n - 1) { + double dt = t[n - 1] - t[n - 2]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[i] = (xs[i] - xs[i - 1]) / dt; + vy[i] = (ys[i] - ys[i - 1]) / dt; + vth[i] = wrap(th[i] - th[i - 1]) / dt; + } else { + double dt = t[i + 1] - t[i - 1]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[i] = (xs[i + 1] - xs[i - 1]) / dt; + vy[i] = (ys[i + 1] - ys[i - 1]) / dt; + vth[i] = wrap(th[i + 1] - th[i - 1]) / dt; + } + } + } + + for (int i = 0; i < n; i++) { + int cnt = 0; + double sx = 0, sy = 0, st = 0; + for (int j = i - 1; j <= i + 1; j++) { + if (j < 0 || j >= n) + continue; + sx += vx[j]; + sy += vy[j]; + st += vth[j]; + cnt++; + } + double svx = sx / cnt, svy = sy / cnt, svt = st / cnt; + JSONObject p = raw.getJSONObject(i); + JSONObject c = new JSONObject(); + c.put("x", p.optDouble("x", 0.0)); + c.put("y", p.optDouble("y", 0.0)); + c.put("angle", p.optDouble("angle", 0.0)); + c.put("time", p.optDouble("time", i)); + c.put("x_velocity", svx); + c.put("y_velocity", svy); + c.put("angular_velocity", svt); + out.put(c); + } + return out; + } + + private double wrap(double a) { + while (a > Math.PI) + a -= 2 * Math.PI; + while (a < -Math.PI) + a += 2 * Math.PI; + return a; + } + + private boolean insideRadius(double ax, double ay, double atheta, double radius) { + double norm = Math.sqrt(ax * ax + ay * ay + atheta * atheta); + return norm <= radius; + } + + @Override + public void initialize() { + currentIndex = 0; + if (rawPoints.length() > 0 && !odometrySet) { + JSONObject first = rawPoints.getJSONObject(0); + drive.setOdometry(new Pose2d(first.optDouble("x", 0.0), first.optDouble("y", 0.0), + new Rotation2d(first.optDouble("angle", 0.0)))); + odometrySet = true; + } + } + + @Override + public void execute() { + if (pointsWithVels.length() == 0) + return; + + Pose2d pose = drive.getMT2Odometry(); + double currentX = pose.getX(); + double currentY = pose.getY(); + double currentTheta = pose.getRotation().getRadians(); + + JSONObject last = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + double dxFinal = last.getDouble("x") - currentX; + double dyFinal = last.getDouble("y") - currentY; + double dthetaFinal = wrap(last.getDouble("angle") - currentTheta); + if (Math.hypot(dxFinal, dyFinal) < POSITION_TOLERANCE && Math.abs(dthetaFinal) < ANGLE_TOLERANCE) { + drive.stop(); + return; // skip all velocity calculations + } + + JSONObject targetPoint = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + int targetIndex = pointsWithVels.length() - 1; + + for (int i = currentIndex; i < pointsWithVels.length(); i++) { + JSONObject point = pointsWithVels.getJSONObject(i); + double targetX = point.getDouble("x"); + double targetY = point.getDouble("y"); + double targetTheta = point.getDouble("angle"); + double targetXvel = point.optDouble("x_velocity", 0.0); + double targetYvel = point.optDouble("y_velocity", 0.0); + double targetThetavel = point.optDouble("angular_velocity", 0.0); + + targetTheta = wrap(targetTheta - currentTheta) + currentTheta; + double linearVelMag = Math.hypot(targetYvel, targetXvel); + double targetVelMag = Math.hypot(linearVelMag, targetThetavel); + double lookaheadRadius = 0.6 * targetVelMag + 0.2; + double deltaX = currentX - targetX; + double deltaY = currentY - targetY; + double deltaTheta = currentTheta - targetTheta; + + if (!insideRadius(deltaX, deltaY, deltaTheta, lookaheadRadius)) { + targetIndex = i; + targetPoint = pointsWithVels.getJSONObject(i); + break; + } + } + + double targetX = targetPoint.getDouble("x"); + double targetY = targetPoint.getDouble("y"); + double targetTheta = wrap(targetPoint.getDouble("angle") - currentTheta) + currentTheta; + + double errX = targetX - currentX; + double errY = targetY - currentY; + double errTheta = wrap(targetTheta - currentTheta); + + double xVelNoFF = errX * kP_x; + double yVelNoFF = errY * kP_y; + double thetaVelNoFF = -errTheta * kP_theta; + + double feedForwardX = 0; + double feedForwardY = 0; + double feedForwardTheta = 0; + + double finalX = xVelNoFF + feedForwardX; + double finalY = yVelNoFF + feedForwardY; + double finalTheta = (thetaVelNoFF + feedForwardTheta) * 1.25; + + double cos = Math.cos(-currentTheta); + double sin = Math.sin(-currentTheta); + double robotX = finalX * cos - finalY * sin; + double robotY = finalX * sin + finalY * cos; + + drive.autoDrive(new Vector(robotX, -robotY), finalTheta); + currentIndex = Math.max(currentIndex, Math.min(targetIndex, pointsWithVels.length() - 1)); + + Logger.recordOutput("PurePursuit/targetIndex", targetIndex); + Logger.recordOutput("PurePursuit/currentIndex", currentIndex); + Logger.recordOutput("PurePursuit/targetPose", new Pose2d(targetX, targetY, new Rotation2d(targetTheta))); + Logger.recordOutput("PurePursuit/vel_field", new double[] { finalX, finalY, finalTheta }); + Logger.recordOutput("PurePursuit/vel_robot", new double[] { robotX, robotY }); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + } + + @Override + public boolean isFinished() { + if (pointsWithVels.length() == 0) + return true; + Pose2d pose = drive.getMT2Odometry(); + JSONObject last = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + double dx = last.getDouble("x") - pose.getX(); + double dy = last.getDouble("y") - pose.getY(); + double dtheta = wrap(last.getDouble("angle") - pose.getRotation().getRadians()); + return Math.hypot(dx, dy) < POSITION_TOLERANCE && Math.abs(dtheta) < ANGLE_TOLERANCE; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java b/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java new file mode 100644 index 0000000..cc24041 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import frc.robot.subsystems.Drive; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; + +public class SetOdometry extends InstantCommand { + public SetOdometry(JSONObject args, Drive drive) { + super(() -> { + if (args == null) { + Logger.recordOutput("Auto/OdoArgsNull", 1); + return; + } + + double x = args.optDouble("x", 0.0); + double y = args.optDouble("y", 0.0); + double angle = args.optDouble("angle", 0.0); + + try { + drive.setOdometry(new Pose2d(x, y, Rotation2d.fromDegrees(angle))); + Logger.recordOutput("Auto/OdoX", x); + Logger.recordOutput("Auto/OdoY", y); + Logger.recordOutput("Auto/OdoAngle", angle); + } catch (Exception e) { + Logger.recordOutput("Auto/OdoSetError", 1); + } + }); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java new file mode 100644 index 0000000..c731e60 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java @@ -0,0 +1,50 @@ +// 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; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotState extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotState(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + superstructure.setWantedState(state); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (DriverStation.isAutonomousEnabled()) { + superstructure.setWantedState(SuperState.IDLE); + } else { + superstructure.setWantedState(SuperState.DEFAULT); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java new file mode 100644 index 0000000..9accf6a --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java @@ -0,0 +1,47 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateComplicated extends Command { + Superstructure superstructure; + SuperState startState; + SuperState endState; + + /** Creates a new SetShootingState. */ + public SetRobotStateComplicated(Superstructure superstructure, SuperState startState, SuperState endState) { + this.superstructure = superstructure; + this.startState = startState; + this.endState = endState; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(startState); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + superstructure.setWantedState(endState); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java new file mode 100644 index 0000000..b9a6a76 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java @@ -0,0 +1,48 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateComplicatedContinuous extends Command { + Superstructure superstructure; + SuperState startState; + SuperState endState; + + /** Creates a new SetShootingState. */ + public SetRobotStateComplicatedContinuous(Superstructure superstructure, SuperState startState, + SuperState endState) { + this.superstructure = superstructure; + this.startState = startState; + this.endState = endState; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + superstructure.setWantedState(startState); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + superstructure.setWantedState(endState); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java new file mode 100644 index 0000000..d1d6193 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.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; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateOnce extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotStateOnce(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(state); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("Interrupted? " + interrupted); + if (DriverStation.isAutonomousEnabled()) { + superstructure.setWantedState(SuperState.IDLE); + } else { + superstructure.setWantedState(SuperState.DEFAULT); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java new file mode 100644 index 0000000..770df11 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java @@ -0,0 +1,47 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SetRobotStateSimple extends Command { + /** Creates a new L3Setup. */ + Superstructure superstructure; + SuperState superState; + + public SetRobotStateSimple(Superstructure superstructure, SuperState superState) { + this.superstructure = superstructure; + this.superState = superState; + addRequirements(this.superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(superState); + System.out.println("Robot State Updating: " + superState); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java new file mode 100644 index 0000000..b7d354f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java @@ -0,0 +1,46 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateSimpleOnce extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotStateSimpleOnce(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(state); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/Trigger.java b/2026-Robot/src/main/java/frc/robot/commands/Trigger.java new file mode 100644 index 0000000..d659176 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Trigger.java @@ -0,0 +1,37 @@ +// 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; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** Add your docs here. */ +public class Trigger { + private BooleanSupplier condition; + private List commands = new ArrayList<>(); + + public Trigger(BooleanSupplier condition, Command... commands) { + this.condition = condition; + for (Command command : commands) { + this.commands.add(command); + } + } + + private void runCommands() { + for (Command command : commands) { + CommandScheduler.getInstance().schedule(command); + } + } + + private void check() { + if (condition.getAsBoolean()) { + runCommands(); + } + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java b/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java new file mode 100644 index 0000000..41e9094 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java @@ -0,0 +1,41 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Peripherals; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ZeroPigeon extends Command { + /** Creates a new ZeroPigeon. */ + Peripherals peripheralSubsystem; + + public ZeroPigeon(Peripherals peripherals) { + peripheralSubsystem = peripherals; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + peripheralSubsystem.zeroPigeon(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java new file mode 100644 index 0000000..e2b5c94 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java @@ -0,0 +1,17 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public interface Conditional { + + boolean evaluate(); + + Command onTrue(); + + Command onFalse(); + + void setArguments(JSONObject args); + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java new file mode 100644 index 0000000..f11ad0b --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java @@ -0,0 +1,38 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; +import edu.wpi.first.wpilibj2.command.Command; + +public class LeftPath implements Conditional { + + private double meow = 0; + + public LeftPath() { + + } + + @Override + public boolean evaluate() { + System.out.println("Evaluating LeftPath Conditional: " + (meow == 67)); + return meow == 67; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + if (args != null) { + if (args.has("value")) { + meow = args.getDouble("value"); + } + } + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java new file mode 100644 index 0000000..4f58363 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java @@ -0,0 +1,29 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public class StartMainPath implements Conditional { + + @Override + public boolean evaluate() { + return false; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java new file mode 100644 index 0000000..be2434d --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java @@ -0,0 +1,28 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public class StartRightPath implements Conditional { + + @Override + public boolean evaluate() { + return false; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java new file mode 100644 index 0000000..b223a3c --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java @@ -0,0 +1,4441 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.OI; +import frc.robot.tools.math.PID; +import frc.robot.tools.math.Vector; + +// **Zero Wheels with the bolt head showing on the left when the front side(battery) is facing down/away from you** + +public class Drive extends SubsystemBase { + private final TalonFX frontRightDriveMotor = new TalonFX(Constants.CANInfo.FRONT_RIGHT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontRightAngleMotor = new TalonFX(Constants.CANInfo.FRONT_RIGHT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontLeftDriveMotor = new TalonFX(Constants.CANInfo.FRONT_LEFT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontLeftAngleMotor = new TalonFX(Constants.CANInfo.FRONT_LEFT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backLeftDriveMotor = new TalonFX(Constants.CANInfo.BACK_LEFT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backLeftAngleMotor = new TalonFX(Constants.CANInfo.BACK_LEFT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backRightDriveMotor = new TalonFX(Constants.CANInfo.BACK_RIGHT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backRightAngleMotor = new TalonFX(Constants.CANInfo.BACK_RIGHT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + + private final CANcoder frontRightCanCoder = new CANcoder(Constants.CANInfo.FRONT_RIGHT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder frontLeftCanCoder = new CANcoder(Constants.CANInfo.FRONT_LEFT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder backLeftCanCoder = new CANcoder(Constants.CANInfo.BACK_LEFT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder backRightCanCoder = new CANcoder(Constants.CANInfo.BACK_RIGHT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + + public boolean getSwerveCAN() { // checks to see if all of the swerve motors and encoders are connected + if (getSwerveMotorsConnected() == 8 && getSwerveCANCodersConnected() == 4) { + return true; + } else + return false; + } + + public int getSwerveMotorsConnected() { // counts all of the swerve motors that are connected + int count = 0; + if (frontRightDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontLeftDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backRightDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backLeftDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontRightAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontLeftAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backRightAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backLeftAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + return count; + } + + public int getSwerveCANCodersConnected() { // counts all of the swerve encoders that are connected + int count = 0; + if (frontRightCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (frontLeftCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (backRightCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (backLeftCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + return count; + } + + // creates all 4 modules + private final SwerveModule frontRight = new SwerveModule(1, frontRightAngleMotor, frontRightDriveMotor, + frontRightCanCoder); + private final SwerveModule frontLeft = new SwerveModule(2, frontLeftAngleMotor, frontLeftDriveMotor, + frontLeftCanCoder); + private final SwerveModule backLeft = new SwerveModule(3, backLeftAngleMotor, backLeftDriveMotor, backLeftCanCoder); + private final SwerveModule backRight = new SwerveModule(4, backRightAngleMotor, backRightDriveMotor, + backRightCanCoder); + + Peripherals peripherals; + boolean firstClimb = false; + + // xy position of module based on robot width and distance from edge of robot + private final double moduleX = ((Constants.Physical.ROBOT_LENGTH) / 2) - Constants.Physical.MODULE_OFFSET; + private final double moduleY = ((Constants.Physical.ROBOT_WIDTH) / 2) - Constants.Physical.MODULE_OFFSET; + + // Locations for the swerve drive modules relative to the robot center. + Translation2d m_frontLeftLocation = new Translation2d(moduleX, moduleY); + Translation2d m_frontRightLocation = new Translation2d(moduleX, -moduleY); + Translation2d m_backLeftLocation = new Translation2d(-moduleX, moduleY); + Translation2d m_backRightLocation = new Translation2d(-moduleX, -moduleY); + + // odometry + + private double m_initTime; + private double m_currentTime; + + // Creating my kinematics object using the module locations + SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + + SwerveDrivePoseEstimator mt2Odometry; + Pose2d mt2Pose; + + PhotonPoseEstimator photonPoseEstimator; + PhotonPoseEstimator backPhotonPoseEstimator; + PhotonPoseEstimator backLeftPhotonPoseEstimator; + PhotonPoseEstimator backRightPhotonPoseEstimator; + + // PhotonPoseEstimator rightPhotonPoseEstimator; + // PhotonPoseEstimator leftPhotonPoseEstimator; + PhotonPoseEstimator swervePhotonPoseEstimator; + PhotonPoseEstimator gamePiecePhotonPoseEstimator; + AprilTagFieldLayout aprilTagFieldLayout; + + // *********************NOTE THE PITCH IS POSITIVE DOWNWARDS + // ********************************** + + Transform3d frontReefRobotToCam = new Transform3d( // top front reef cam + new Translation3d(Constants.inchesToMeters(2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(23.625)), + new Rotation3d(Math.toRadians(0.3), Math.toRadians(25.6), Math.toRadians(15.0))); + + Transform3d frontSwerveRobotToCam = new Transform3d( // front reef cam on swerve module + new Translation3d(Constants.inchesToMeters(11.75), + Constants.inchesToMeters(-8.5), + Constants.inchesToMeters(8.75)), + new Rotation3d(Math.toRadians(1.1), Math.toRadians(15.4), + Math.toRadians(35.0))); + + Transform3d backReefRobotToCam = new Transform3d( // top back reef cam + new Translation3d(Constants.inchesToMeters(-2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(23.625)), + new Rotation3d(Math.toRadians(2.8), Math.toRadians(25.9), Math.toRadians(165.0))); + + Transform3d backLeftReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(6.4), Constants.inchesToMeters(7.0), + Constants.inchesToMeters(18.7)), + new Rotation3d(Math.toRadians(0.2), Math.toRadians(10.1), Math.toRadians(0.0))); // 0.4, -20.5 + + Transform3d backRightReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters( + 6.4), Constants.inchesToMeters(-7.0), + Constants.inchesToMeters(18.7)), + new Rotation3d(Math.toRadians(0.4), Math.toRadians(10.6), Math.toRadians(0.0))); + + Transform3d gamePieceReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(20.25)), + new Rotation3d(Math.toRadians(1.0), Math.toRadians(21.4), Math.toRadians(15.0))); + + double initAngle; + double setAngle; + double diffAngle; + + // path following PID values + private double kXP = 4.00; + private double kXI = 0.00; + private double kXD = 1.20; + + private double kYP = kXP; + private double kYI = kXI; + private double kYD = kXD; + + private double kThetaP = 2.90; + private double kThetaI = 0.00; + private double kThetaD = 2.00; + + // auto placement PID values + private double kkXP = 5.50; + private double kkXI = 0.00; + private double kkXD = 1.60; + + private double kkYP = kkXP; + private double kkYI = kkXI; + private double kkYD = kkXD; + + private double kkThetaP = 1.79; + private double kkThetaI = 0.00; + private double kkThetaD = 0.971; + + // l4 pid values + private double kkXP4 = 4.30; + private double kkXI4 = 0.00; + private double kkXD4 = 1.70; + + private double kkYP4 = kkXP4; + private double kkYI4 = kkXI4; + private double kkYD4 = kkXD4; + + private double kkThetaP4 = 2.056; + private double kkThetaI4 = 0.00; + private double kkThetaD4 = 0.971; + + // l4 auto pids + private double scalar = 0.9; + private double kkXP4A = 4.30 * scalar; + private double kkXI4A = 0.00 * scalar; + private double kkXD4A = 1.70 * scalar; + + private double kkYP4A = kkXP4A; + private double kkYI4A = kkXI4A; + private double kkYD4A = kkXD4A; + + private double kkThetaP4A = 2.056; + private double kkThetaI4A = 0.00; + private double kkThetaD4A = 0.971; + + // l23 pid values + private double kkXP23 = 4.00; + private double kkXI23 = 0.00; + private double kkXD23 = 1.60; + + private double kkYP23 = kkXP23; + private double kkYI23 = kkXI23; + private double kkYD23 = kkXD23; + + private double kkThetaP23 = 2.481; + private double kkThetaI23 = 0.00; + private double kkThetaD23 = 0.971; + + // l1 pid values + private double kkXP1 = 4.00; + private double kkXI1 = 0.00; + private double kkXD1 = 1.40; + + private double kkYP1 = kkXP1; + private double kkYI1 = kkXI1; + private double kkYD1 = kkXD1; + + private double kkThetaP1 = 3.005; + private double kkThetaI1 = 0.00; + private double kkThetaD1 = 0.971; + + // Piece pickup values + private double kkkXPPickup = 3.30; + private double kkkXIPickup = 0.00; + private double kkkXDPickup = 1.70; + + private double kkkYPPickup = kkkXPPickup; + private double kkkYIPickup = kkkXIPickup; + private double kkkYDPickup = kkkXDPickup; + + private double kkkThetaPPickup = 2.056; + private double kkkThetaIPickup = 0.00; + private double kkkThetaDPickup = 0.971; + + // teleop targeting PID values + private double kTurningP = 0.04; + private double kTurningI = 0; + private double kTurningD = 0.06; + private double kRotateP = 0.04; + private double kRotateI = 0.0; + private double kRotateD = 0.06; + + private PID xxPID = new PID(kkXP, kkXI, kkXD); + private PID yyPID = new PID(kkYP, kkYI, kkYD); + private PID thetaaPID = new PID(kkThetaP, kkThetaI, kkThetaD); + + private PID xxPID4 = new PID(kkXP4, kkXI4, kkXD4); + private PID yyPID4 = new PID(kkYP4, kkYI4, kkYD4); + private PID thetaaPID4 = new PID(kkThetaP4, kkThetaI4, kkThetaD4); + + private PID xxPID4A = new PID(kkXP4A, kkXI4A, kkXD4A); + private PID yyPID4A = new PID(kkYP4A, kkYI4A, kkYD4A); + private PID thetaaPID4A = new PID(kkThetaP4A, kkThetaI4A, kkThetaD4A); + + private PID xxPID23 = new PID(kkXP23, kkXI23, kkXD23); + private PID yyPID23 = new PID(kkYP23, kkYI23, kkYD23); + private PID thetaaPID23 = new PID(kkThetaP23, kkThetaI23, kkThetaD23); + + private PID xxPID1 = new PID(kkXP1, kkXI1, kkXD1); + private PID yyPID1 = new PID(kkYP1, kkYI1, kkYD1); + private PID thetaaPID1 = new PID(kkThetaP1, kkThetaI1, kkThetaD1); + + private PID xxPIDPickup = new PID(kkkXPPickup, kkkXIPickup, kkkXDPickup); + private PID yyPIDPickup = new PID(kkkYPPickup, kkkYIPickup, kkkYDPickup); + private PID thetaaPIDPickup = new PID(kkkThetaPPickup, kkkThetaIPickup, kkkThetaDPickup); + + private PID xPID = new PID(kXP, kXI, kXD); + private PID yPID = new PID(kYP, kYI, kYD); + private PID thetaPID = new PID(kThetaP, kThetaI, kThetaD); + private PID turningPID = new PID(kTurningP, kTurningI, kTurningD); + private PID rotatePID = new PID(kRotateP, kRotateI, kRotateD); + + private String m_fieldSide = "blue"; + + private double angleSetpoint = 0; + double startX; + double startY; + + public boolean algaeMode = false; + + public enum DriveState { + DEFAULT, + IDLE, + STOP, + REEF, + REEF_MORE, + BACK, + L3_REEF, + L4_REEF, + ALGAE, + ALGAE_MORE, + ALGAE_MORE_MORE, + PROCESSOR, + PROCESSOR_MORE, + NET, + NET_MORE, + FEEDER, + SCORE_L23, + FEEDER_ALIGN, + AUTO_FEEDER, + AUTO_L1, + AUTO_L1_MORE, + FEEDER_AUTO, + PIECE_PICKUP, + AUTO_CLIMB + } + + private DriveState wantedState = DriveState.IDLE; + private DriveState systemState = DriveState.IDLE; + + /** + * Creates a new instance of the Swerve Drive subsystem. + * Initializes the Swerve Drive subsystem with the provided peripherals. + * + * @param peripherals The peripherals used by the Swerve Drive subsystem. + */ + + public Drive(Peripherals peripherals) { + this.peripherals = peripherals; + + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(0, new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(0, new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(0, new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(0, new Rotation2d(backRight.getCanCoderPositionRadians())); + + Pose2d m_pose = new Pose2d(); + mt2Odometry = new SwerveDrivePoseEstimator(m_kinematics, + new Rotation2d(Math.toRadians(peripherals.getPigeonAngle())), swerveModulePositions, m_pose); + } + + // public boolean atSetpoint() { + // double currentAngle = peripherals.getPigeonAngle(); + // if (getFieldSide().equals("red")) { + // currentAngle -= 180; + // } + // return (Math.abs(Constants.standardizeAngleDegrees(currentAngle) + // - Constants.standardizeAngleDegrees(angleSetpoint)) < 2); + // } + + public void setWantedState(DriveState wantedState) { + this.wantedState = wantedState; + } + + public void setWantedState(DriveState wantedState, double angle) { + this.wantedState = wantedState; + setSetpointAngle(angle); + } + + public void setSetpointAngle(double angle) { + this.angleSetpoint = angle; + // Logger.recordOutput("Setpoint Theta: ", angle); + } + + /** + * Initializes the robot with the specified field side configuration. + * It sets up configurations when run on robot initialization, such as setting + * the field side, + * initializing each swerve module, configuring motor inversions, and setting + * PID controller output limits. + * Additionally, it sets the default command to the DriveDefault command. + * + * @param fieldSide The side of the field (e.g., "red" or "blue"). + */ + public void init(String fieldSide) { + // sets configurations when run on robot initalization + this.m_fieldSide = fieldSide; + + try { + aprilTagFieldLayout = new AprilTagFieldLayout( + Filesystem.getDeployDirectory().getPath() + "/" + "2025-reefscape-andymark.json"); + } catch (Exception e) { + java.util.logging.Logger.getGlobal().warning("error with april tag: " + e.getMessage()); + } + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontReefRobotToCam); + + backPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backReefRobotToCam); + + backLeftPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backLeftReefRobotToCam); + + backRightPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backRightReefRobotToCam); + + swervePhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontSwerveRobotToCam); + + // rightPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + // PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontBargeRobotToCam); + + // leftPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + // PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backBargeRobotToCam); + + gamePiecePhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, gamePieceReefRobotToCam); + + frontRight.init(); + frontLeft.init(); + backRight.init(); + backLeft.init(); + + xxPID.setMinOutput(-3.0); + xxPID.setMaxOutput(3.0); + + yyPID.setMinOutput(-3.0); + yyPID.setMaxOutput(3.0); + + thetaaPID.setMinOutput(-3.0); + thetaaPID.setMaxOutput(3.0); + + xxPID4.setMinOutput(-2.0); + xxPID4.setMaxOutput(2.0); + + yyPID4.setMinOutput(-2.0); + yyPID4.setMaxOutput(2.0); + + thetaaPID4.setMinOutput(-2.0); + thetaaPID4.setMaxOutput(2.0); + + xxPID4A.setMinOutput(-1.4); + xxPID4A.setMaxOutput(1.4); + + yyPID4A.setMinOutput(-1.4); + yyPID4A.setMaxOutput(1.4); + + thetaaPID4A.setMinOutput(-1.4); + thetaaPID4A.setMaxOutput(1.4); + + xxPID23.setMinOutput(-4.0); + xxPID23.setMaxOutput(4.0); + + yyPID23.setMinOutput(-4.0); + yyPID23.setMaxOutput(4.0); + + thetaaPID23.setMinOutput(-4.0); + thetaaPID23.setMaxOutput(4.0); + + xxPID1.setMinOutput(-4.0); + xxPID1.setMaxOutput(4.0); + + yyPID1.setMinOutput(-4.0); + yyPID1.setMaxOutput(4.0); + + thetaaPID1.setMinOutput(-4.0); + thetaaPID1.setMaxOutput(4.0); + + xxPIDPickup.setMinOutput(-1.0); + xxPIDPickup.setMaxOutput(1.0); + + yyPIDPickup.setMinOutput(-1.0); + yyPIDPickup.setMaxOutput(1.0); + + thetaaPIDPickup.setMinOutput(-2.0); + thetaaPIDPickup.setMaxOutput(2.0); + + xPID.setMinOutput(-4.9); + xPID.setMaxOutput(4.9); + + yPID.setMinOutput(-4.9); + yPID.setMaxOutput(4.9); + + thetaPID.setMinOutput(-3); + thetaPID.setMaxOutput(3); + + turningPID.setMinOutput(-3); + turningPID.setMaxOutput(3); + + rotatePID.setMinOutput(-2); + rotatePID.setMaxOutput(2); + } + + public void teleopInit() { + angleSetpoint = peripherals.getPigeonAngle(); + if (getFieldSide() == "red") { + angleSetpoint -= 180; + } + turningPID.setSetPoint(angleSetpoint); + if (Math.abs(turningPID.getSetPoint() - angleSetpoint) > 100) { + turningPID.setSetPoint(angleSetpoint); + } + + frontLeft.setDriveCurrentLimits(120, 120); // if robot starts browning out in a placement sequence, just drop + // these + // to 40 + frontRight.setDriveCurrentLimits(120, 120); + backLeft.setDriveCurrentLimits(120, 120); + backRight.setDriveCurrentLimits(120, 120); + } + + public void teleopPeriodic() { + if (Math.abs(turningPID.getSetPoint() - angleSetpoint) > 100) { + turningPID.setSetPoint(angleSetpoint); + } + } + + /** + * Zeros the IMU (Inertial Measurement Unit) mid-match and resets the odometry + * with a zeroed angle. + * It resets the angle reported by the pigeon sensor to zero and updates the + * odometry with this new zeroed angle. + */ + public void zeroIMU() { + angleSetpoint = 0; + turningPID.setSetPoint(angleSetpoint); + peripherals.zeroPigeon(); + // SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + // swerveModulePositions[0] = new + // SwerveModulePosition(frontLeft.getModuleDistance(), + // new Rotation2d(frontLeft.getCanCoderPositionRadians())); + // swerveModulePositions[1] = new + // SwerveModulePosition(frontRight.getModuleDistance(), + // new Rotation2d(frontRight.getCanCoderPositionRadians())); + // swerveModulePositions[2] = new + // SwerveModulePosition(backLeft.getModuleDistance(), + // new Rotation2d(backLeft.getCanCoderPositionRadians())); + // swerveModulePositions[3] = new + // SwerveModulePosition(backRight.getModuleDistance(), + // new Rotation2d(backRight.getCanCoderPositionRadians())); + + // m_pose = m_odometry.update(new + // Rotation2d((Math.toRadians(peripherals.getPigeonAngle()))), + // swerveModulePositions); + // loggingPose = loggingOdometry.update(new Rotation2d( + // Math.toRadians(peripherals.getPigeonAngle())), swerveModulePositions); + // mt2Pose = mt2Odometry.update(new + // Rotation2d(Math.toRadians(peripherals.getPigeonAngle())), + // swerveModulePositions); + } + + /** + * Adjusts the angle reported by the pigeon sensor after an autonomous routine. + * It adds 180 degrees to the current angle reported by the pigeon sensor and + * wraps it around 360 degrees. + */ + public void setPigeonAfterAuto() { + peripherals.setPigeonAngle((peripherals.getPigeonAngle() + 180) % 360); + } + + /** + * Sets the angle reported by the pigeon sensor to the specified value. + * + * @param angle The angle to set for the pigeon sensor in degrees. + */ + public void setPigeonAngle(double angle) { + peripherals.setPigeonAngle(angle); + } + + /** + * Retrieves the current angle reported by the pigeon sensor. + * + * @return The current angle reported by the pigeon sensor in degrees. + */ + public double getPigeonAngle() { + return peripherals.getPigeonAngle(); + } + + /** + * Sets the PID values for all swerve modules to zero, keeping the wheels + * straight. + */ + public void setWheelsStraight() { + frontRight.setWheelPID(0.0, 0.0); + frontLeft.setWheelPID(0.0, 0.0); + backLeft.setWheelPID(0.0, 0.0); + backRight.setWheelPID(0.0, 0.0); + } + + /** + * Initializes the robot's state for autonomous mode based on the provided path + * points. + * + * @param pathPoints The array of path points representing the trajectory for + * the autonomous routine. + */ + public void autoInit(JSONArray pathPoints) { + // runs at start of autonomous + java.util.logging.Logger.getGlobal().info("Auto init"); + JSONObject firstPoint = pathPoints.getJSONObject(0); + double firstPointX = firstPoint.getDouble("x"); + double firstPointY = firstPoint.getDouble("y"); + double firstPointAngle = firstPoint.getDouble("angle"); + + // changing odometry if on red side, don't need to change y because it will be + // the same for autos on either side + if (this.m_fieldSide == "blue") { + firstPointX = Constants.Physical.FIELD_LENGTH - firstPointX; + firstPointY = Constants.Physical.FIELD_WIDTH - firstPointY; + firstPointAngle = Math.PI + firstPointAngle; + } + + frontLeft.setDriveCurrentLimits(60, 120); + frontRight.setDriveCurrentLimits(60, 120); + backLeft.setDriveCurrentLimits(60, 120); + backRight.setDriveCurrentLimits(60, 120); + + peripherals.setPigeonAngle(Math.toDegrees(firstPointAngle)); + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(frontLeft.getModuleDistance(), + new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(frontRight.getModuleDistance(), + new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(backLeft.getModuleDistance(), + new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(backRight.getModuleDistance(), + new Rotation2d(backRight.getCanCoderPositionRadians())); + mt2Odometry.resetPosition(new Rotation2d(firstPointAngle), swerveModulePositions, + new Pose2d(new Translation2d(firstPointX, firstPointY), new Rotation2d(firstPointAngle))); + + m_initTime = Timer.getFPGATimestamp(); + + updateOdometryFusedArray(); + } + + /** + * Sets the current field side designation. + * + * @param side The field side designation to set, indicating whether the robot + * is positioned on the "blue" or "red" side of the field. + */ + public void setFieldSide(String side) { + m_fieldSide = side; + } + + /** + * Retrieves the current field side designation. + * + * @return The current field side designation, indicating whether the robot is + * positioned on the "blue" or "red" side of the field. + */ + public String getFieldSide() { + return m_fieldSide; + } + + /** + * Retrieves the current timestamp relative to the start of the robot operation. + * + * @return The current timestamp in seconds since the start of the robot + * operation. + */ + public double getCurrentTime() { + return m_currentTime; + } + + public void setOdometry(Pose2d pose) { + // Logger.recordOutput("Odometry Reset to:", pose.toString()); + java.util.logging.Logger.getGlobal().fine("New Odometry Pose: " + pose.toString()); + mt2Odometry.resetPose(pose); + } + + public boolean isPoseInField(Pose2d pose) { + if (pose.getY() < 0 || pose.getY() > Constants.Physical.FIELD_WIDTH || pose.getX() < 0 + || pose.getX() > Constants.Physical.FIELD_LENGTH) { + return false; + } else { + return true; + } + } + + private int getClosestTagId(Pose2d pose) { + int closestTag = 0; + double closestDistance = Double.MAX_VALUE; + + for (int i = 1; i <= aprilTagFieldLayout.getTags().size(); i++) { + Optional tagPose = aprilTagFieldLayout.getTagPose(i); + if (tagPose.isPresent()) { + double distance = Constants.Vision.distBetweenPose2d(pose, tagPose.get().toPose2d()); + if (distance < closestDistance) { + closestDistance = distance; + closestTag = i; + } + } + } + return closestTag; + } + + private boolean inReefInteractionState() { + return systemState == DriveState.L4_REEF || systemState == DriveState.L3_REEF || + systemState == DriveState.REEF || systemState == DriveState.AUTO_L1 || + systemState == DriveState.AUTO_L1_MORE || systemState == DriveState.ALGAE || + systemState == DriveState.ALGAE_MORE || systemState == DriveState.ALGAE_MORE_MORE; + } + + private boolean closeToReef() { + double dist = DriverStation.isAutonomousEnabled() ? 2.7 : 2.3; + if (distanceFromCenterOfReef() < dist) { + return true; + } else { + return false; + } + } + + /** + * Updates the fused odometry array with current robot position and orientation + * information. + * Calculates the robot's position and orientation using swerve module positions + * and the gyro angle. + * Updates the current X, Y, and theta values, as well as previous values and + * time differences. + */ + public void updateOdometryFusedArray() { + double navxOffset = Math.toRadians(peripherals.getPigeonAngle()); + + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(frontLeft.getModuleDistance(), + new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(frontRight.getModuleDistance(), + new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(backLeft.getModuleDistance(), + new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(backRight.getModuleDistance(), + new Rotation2d(backRight.getCanCoderPositionRadians())); + mt2Pose = mt2Odometry.update(new Rotation2d(navxOffset), swerveModulePositions); + + Matrix standardDeviation = new Matrix<>(Nat.N3(), Nat.N1()); + Logger.recordOutput("Closde to reef", closeToReef()); + + if (((closeToReef()) || inReefInteractionState()) + && systemState != DriveState.REEF_MORE) { + photonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backLeftPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backRightPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + swervePhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + gamePiecePhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + + Rotation2d robotRotation = new Rotation2d(navxOffset); + double time = Timer.getFPGATimestamp(); + photonPoseEstimator.addHeadingData(time, robotRotation); + backPhotonPoseEstimator.addHeadingData(time, robotRotation); + backLeftPhotonPoseEstimator.addHeadingData(time, robotRotation); + backRightPhotonPoseEstimator.addHeadingData(time, robotRotation); + swervePhotonPoseEstimator.addHeadingData(time, robotRotation); + gamePiecePhotonPoseEstimator.addHeadingData(time, robotRotation); + } + Logger.recordOutput("Back Strategy: ", + backPhotonPoseEstimator.getPrimaryStrategy().toString()); + Logger.recordOutput("Back left strat: ", backLeftPhotonPoseEstimator.getPrimaryStrategy().toString()); + Logger.recordOutput("Back right strat: ", + backRightPhotonPoseEstimator.getPrimaryStrategy().toString()); + + if (getRobotSpeed() < 2.4) { + var backResult = peripherals.getBackReefCamResult(); + Optional backMultiTagResult = backPhotonPoseEstimator.update(backResult); + if (backMultiTagResult.isPresent()) { + if (backResult.getBestTarget().getPoseAmbiguity() < 0.3 && backResult.getBestTarget().fiducialId != 5 + && backResult.getBestTarget().fiducialId != 4 && backResult.getBestTarget().fiducialId != 14 + && backResult.getBestTarget().fiducialId != 15 && backResult.getBestTarget().fiducialId != 3 + && backResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backMultiTagResult.get().estimatedPose; + Logger.recordOutput("multitag result", robotPose); + int numFrontTracks = backResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backResult.getBestTarget().getFiducialId()).get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var swerveResult = peripherals.getFrontSwerveCamResult(); + Optional swerveMultiTagResult = swervePhotonPoseEstimator.update(swerveResult); + if (swerveMultiTagResult.isPresent() + && (!inReefInteractionState() + || getAutoPlacementSideIsFront())) { + if (swerveResult.getBestTarget().getPoseAmbiguity() < 0.3) { + Pose3d robotPose = swerveMultiTagResult.get().estimatedPose; + int numFrontTracks = swerveResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(swerveResult.getBestTarget().getFiducialId()).get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (swerveResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + swerveResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + swerveResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var backLeftResult = peripherals.getBackLeftReefCamResult(); + Optional backLeftMultiTagResult = backLeftPhotonPoseEstimator.update(backLeftResult); + if (backLeftMultiTagResult.isPresent()) { + if (backLeftResult.getBestTarget().getPoseAmbiguity() < 0.3 + && backLeftResult.getBestTarget().fiducialId != 5 + && backLeftResult.getBestTarget().fiducialId != 4 + && backLeftResult.getBestTarget().fiducialId != 14 + && backLeftResult.getBestTarget().fiducialId != 15 + && backLeftResult.getBestTarget().fiducialId != 3 + && backLeftResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backLeftMultiTagResult.get().estimatedPose; + int numFrontTracks = backLeftResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backLeftResult.getBestTarget().getFiducialId()) + .get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backLeftResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backLeftResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backLeftResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var backRightResult = peripherals.getBackRightReefCamResult(); + Optional backRightMultiTagResult = backRightPhotonPoseEstimator.update(backRightResult); + if (backRightMultiTagResult.isPresent()) { + if (backRightResult.getBestTarget().getPoseAmbiguity() < 0.3 + && backRightResult.getBestTarget().fiducialId != 5 + && backRightResult.getBestTarget().fiducialId != 4 + && backRightResult.getBestTarget().fiducialId != 14 + && backRightResult.getBestTarget().fiducialId != 15 + && backRightResult.getBestTarget().fiducialId != 3 + && backRightResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backRightMultiTagResult.get().estimatedPose; + int numFrontTracks = backRightResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backRightResult.getBestTarget().getFiducialId()) + .get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backRightResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backRightResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backRightResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + } + m_currentTime = Timer.getFPGATimestamp() - m_initTime; + } + + /** + * Retrieves the states of the modules (position and ground speed) of the + * robot's swerve drive system. + * + * @return An array containing the states of each wheel module, consisting of: + * front right module position in degrees, + * front right module ground speed in meters per second, + * front left module position in degrees, + * front left module ground speed in meters per second, + * back left module position in degrees, + * back left module ground speed in meters per second, + * back right module position in degrees, + * back right module ground speed in meters per second. + */ + public double[] getModuleStates() { + double[] states = { + frontLeft.getCanCoderPosition() * 360.0, frontLeft.getGroundSpeed(), + frontRight.getCanCoderPosition() * 360.0, frontRight.getGroundSpeed(), + backLeft.getCanCoderPosition() * 360.0, backLeft.getGroundSpeed(), + backRight.getCanCoderPosition() * 360.0, backRight.getGroundSpeed(), + }; + return states; + } + + /** + * Retrieves the setpoints of the modules (angle and drive motors) of the + * robot's swerve drive system. + * + * @return An array containing the setpoints of the angle and drive motors for + * each wheel module, in the order: + * front right angle motor, front right drive motor, + * front left angle motor, front left drive motor, + * back left angle motor, back left drive motor, + * back right angle motor, back right drive motor. + */ + public double[] getModuleSetpoints() { + double[] setpoints = { + frontLeft.getAngleMotorSetpoint() * 360, frontLeft.getDriveMotorSetpoint(), + frontRight.getAngleMotorSetpoint() * 360, frontRight.getDriveMotorSetpoint(), + backLeft.getAngleMotorSetpoint() * 360, backLeft.getDriveMotorSetpoint(), + backRight.getAngleMotorSetpoint() * 360, backRight.getDriveMotorSetpoint(), + }; + return setpoints; + } + + public double getRobotSpeed() { + return (Math.abs(frontLeft.getGroundSpeed()) + Math.abs(frontRight.getGroundSpeed()) + + Math.abs(backLeft.getGroundSpeed()) + + Math.abs(backRight.getGroundSpeed())) / 4.0; + } + + /** + * Retrieves the current velocity of the angle motors of the robot. + * + * @return An array containing the current velocity of the angle motors for + * front right, front left, back left, and back right wheels. + */ + public double[] getAngleMotorVelocity() { + double[] velocity = { + frontRight.getAngleVelocity(), frontLeft.getAngleVelocity(), backLeft.getAngleVelocity(), + backRight.getAngleVelocity() + }; + return velocity; + } + + public Pose2d getMT2Odometry() { + return mt2Odometry.getEstimatedPosition(); + } + + private boolean autoPlacingFront = true; + + public boolean getAutoPlacementSideIsFront() { + return autoPlacingFront; + } + + public double getAngleDifferenceDegrees(double angle1, double angle2) { + double difference = Math.abs(angle1 - angle2) % 360; + return difference > 180 ? 360 - difference : difference; + } + + public boolean isGoingForL3Algae() { + double thetaSetpoint = getAlgaeClosestSetpoint(getMT2Odometry()).getRotation().getRadians(); + if (autoPlacingFront) { + if (!isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 60.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 180.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 300.0) <= 30) { + return true; + } else { + return false; + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 0.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 120.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 240.0) <= 30) { + return true; + } else { + return false; + } + } + } else { + if (!isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 60.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 180.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 300.0) <= 30) { + return false; + } else { + return true; + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 0.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 120.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 240.0) <= 30) { + return false; + } else { + return true; + } + } + } + } + + public Pose2d getAlgaeMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositionsMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositionsMore.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositionsMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getAlgaeMoreMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(Math.toDegrees(currentOdometry.getRotation().getRadians())); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositionsMoreMore.get(i).getX()), 2) + // + Math.pow((y - + // Constants.Reef.redFrontPlacingPositionsMoreMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositionsMoreMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositionsMoreMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getAlgaeClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositions.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositions.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositions.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getReefClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositions.get(i).getX() + + Constants.Reef.redBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositions.get(i).getY() + + Constants.Reef.redBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.redFrontPlacingPositions.get(i).getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.redFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.redFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.redBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.redBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.redFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.redBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositions.get(i).getX() + + Constants.Reef.blueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositions.get(i).getY() + + Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.blueFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.blueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.blueFrontPlacingPositions.get(i) + .getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.blueBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.blueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.blueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getL1ReefClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redL1FrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.redL1BackPlacingPositions.get(i).getX() + + Constants.Reef.redL1BackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redL1BackPlacingPositions.get(i).getY() + + Constants.Reef.redL1BackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redL1FrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redL1FrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redL1BackPlacingPositions.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueL1FrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueL1BackPlacingPositions.get(i).getX() + + Constants.Reef.blueL1BackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueL1BackPlacingPositions.get(i).getY() + + Constants.Reef.blueL1BackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueL1FrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueL1FrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueL1BackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getL1ReefClosestSetpointMore(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redL1FrontPlacingPositionsMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redL1BackPlacingPositionsMore.get(i).getX() + + Constants.Reef.redL1BackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redL1BackPlacingPositionsMore.get(i).getY() + + Constants.Reef.redL1BackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redL1FrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redL1BackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueL1FrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueL1BackPlacingPositionsMore.get(i).getX() + + Constants.Reef.blueL1BackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueL1BackPlacingPositionsMore.get(i).getY() + + Constants.Reef.blueL1BackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueL1FrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueL1FrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueL1BackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositionsMore.get(i).getX() + + Constants.Reef.redBackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositionsMore.get(i).getY() + + Constants.Reef.redBackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositionsMore.get(i).getX() + + Constants.Reef.blueBackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositionsMore.get(i).getY() + + Constants.Reef.blueBackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 2.0) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getReefL44ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l4RedFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l4RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l4BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } else if (notClosest + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX()) < 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getY()) < 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueBackPlacingPositions.get(i).getX()) < 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) < 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L4 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefL3ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l3RedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l3RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l3BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((Math.hypot( + origionalSetpointPose.getX() - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getX(), + origionalSetpointPose.getY() - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getY()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs( + origionalSetpointPose.getX() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L3 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefL33ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l3RedFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l3RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l3BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } else if (notClosest + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX()) < 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getY()) < 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueBackPlacingPositions.get(i).getX()) < 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) < 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L3 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public void stop() { + frontLeftAngleMotor.stopMotor(); + frontRightAngleMotor.stopMotor(); + backLeftAngleMotor.stopMotor(); + backRightAngleMotor.stopMotor(); + frontLeftDriveMotor.stopMotor(); + frontRightDriveMotor.stopMotor(); + backLeftDriveMotor.stopMotor(); + } + + public Pose2d getReefL4ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l4RedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l4RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l4BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((Math.hypot( + origionalSetpointPose.getX() - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getX(), + origionalSetpointPose.getY() - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getY()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs( + origionalSetpointPose.getX() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L4 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + boolean firstTimeAutoPickup = false; + double firstTimePickupAngle = 0.0; + boolean firstTimeCalculated = false; + boolean firstTimeGoingInCalculated = false; + boolean firstTimeGoingIn = false; + boolean hasTrack = false; + + Pose2d targetPose = new Pose2d(); + Pose2d c1 = new Pose2d(); + Pose2d c2 = new Pose2d(); + + public Pose2d getGamePiecePosition() { + if (!firstTimeAutoPickup) { + firstTimePickupAngle = getMT2OdometryAngle(); + firstTimeAutoPickup = true; + } + + double yaw = 0.0; + double pitch = 0.0; + var result = peripherals.getFrontGamePieceCamResult(); + if (result.hasTargets()) { + List tracks = result.getTargets(); + for (int i = 0; i < tracks.size(); i++) { + int id = tracks.get(i).getDetectedObjectClassID(); + if (id == 0) { + tracks.remove(i); + i--; + } + } + if (tracks.isEmpty()) { + return new Pose2d(); + } else { + double minPitch = tracks.get(0).getPitch(); + int index = 0; + for (int i = 1; i < tracks.size(); i++) { + int id = tracks.get(0).getDetectedObjectClassID(); + java.util.logging.Logger.getGlobal().finer("id: " + id); + if (tracks.get(i).getPitch() < minPitch) { + minPitch = tracks.get(i).getPitch(); + index = i; + } + } + pitch = minPitch; + yaw = tracks.get(index).getYaw(); + } + } + // double yFromIntake = 0.0; + + if (yaw != 0.0 && pitch != 0.0) { + java.util.logging.Logger.getGlobal().finer("calculating%"); + double cameraYaw = 15.0; + double limelightXOffset = Constants.inchesToMeters(2.25); + double limelightYOffset = Constants.inchesToMeters(-11.5); + double intakeXOffset = Constants.inchesToMeters(20.5); + double intakeYOffset = Constants.inchesToMeters(3.0); // 4.0 + + double robotX = getMT2OdometryX(); + double robotY = getMT2OdometryY(); + // double robotAngle = getMT2OdometryAngle(); + double robotAngle = firstTimePickupAngle; + Pose2d robotPose = new Pose2d(robotX, robotY, new Rotation2d(robotAngle)); + + // double targetDistance = Math + // .abs((limelightHeight - gamePieceHeight) / + // Math.tan(Math.toRadians(-limelightAngle + pitch))); + double targetDistance = Constants.inchesToMeters(Constants.Vision.getCoralDistanceFromPitch(pitch)); + Logger.recordOutput("Distance to Coral", targetDistance); + double noteY = -(targetDistance * Math.sin(Math.toRadians(-cameraYaw + yaw))); + double noteX = ((targetDistance * Math.cos(Math.toRadians(cameraYaw - yaw)))); + Logger.recordOutput("coral x", noteX); + Logger.recordOutput("coral y", noteY); + double xFromRobot = noteX + limelightXOffset; + double yFromRobot = noteY + limelightYOffset; + Pose2d coralPose = robotPose.transformBy(new Transform2d(xFromRobot, yFromRobot, new Rotation2d())); + + Logger.recordOutput("coral rc x", xFromRobot); + Logger.recordOutput("coral rc y", yFromRobot); + Logger.recordOutput("coral pose", coralPose); + + double xFromIntake = xFromRobot - intakeXOffset; + double yFromIntake = yFromRobot - intakeYOffset; + if (!firstTimeCalculated) { + hasTrack = true; + targetPose = robotPose.transformBy(new Transform2d(xFromIntake, + yFromIntake, new Rotation2d())); + Logger.recordOutput("coral intake x", xFromIntake); + Logger.recordOutput("coral intake y", yFromIntake); + double projectedTx = Math.atan2(Math.tan(Math.toRadians(yaw)), Math.cos(Math.toRadians(pitch))); + java.util.logging.Logger.getGlobal().finer("Projected: " + Math.toDegrees(projectedTx)); + java.util.logging.Logger.getGlobal().finer("actual: " + yaw); + Pose2d intakeFieldPose = robotPose + .transformBy(new Transform2d(intakeXOffset, intakeYOffset, new Rotation2d())); + double angleToPiece = Constants.standardizeAngleDegrees(Math.toDegrees(Math.atan2( + intakeFieldPose.getY() - coralPose.getY(), intakeFieldPose.getX() - coralPose.getX()) + - Math.PI)); + Logger.recordOutput("Angle to Piece", (angleToPiece)); + Logger.recordOutput("Intake field pose", intakeFieldPose); + + double deltaX = coralPose.getX() - intakeFieldPose.getX(); + double deltaY = coralPose.getY() - intakeFieldPose.getY(); + double targetAngle = Math.atan2(deltaY, deltaX); + Pose2d poseFromRobotFront = new Pose2d( + Constants.metersToInches(xFromRobot - Constants.inchesToMeters(16.5)), + Constants.metersToInches(yFromRobot), new Rotation2d()); + Logger.recordOutput("Pose from robot front", poseFromRobotFront); + + // Adjust for the robot's current heading and camera yaw + double requiredTurn = targetAngle - Math.toRadians(cameraYaw); + + // Normalize to range [-π, π] for minimal turning + requiredTurn = (requiredTurn + (2 * Math.PI)) % (2 * Math.PI); + // driveToPoint(targetPose.getX(), targetPose.getY(), + Logger.recordOutput("new angle", Math.toDegrees(requiredTurn)); + // Pose2d targetPose = robotPose + // .transformBy(new Transform2d(xFromIntake, yFromIntake, new + // Rotation2d(requiredTurn))); + Logger.recordOutput("target pickup", targetPose); + + double mx = (robotX + targetPose.getX()) / 2; + double my = (robotY + targetPose.getY()) / 2; + + // Compute the length of AB (hypotenuse) + double abLength = Math + .sqrt(Math.pow(robotX - targetPose.getX(), 2) + Math.pow(robotY - targetPose.getY(), 2)); + + // The expected right triangle height from midpoint to C + double d = Math.sqrt(xFromIntake * xFromIntake + yFromIntake * yFromIntake - (abLength * abLength) / 2); + + // Compute the perpendicular direction (normalized) + double perpX = -(robotY - targetPose.getY()) / abLength; + double perpY = (robotX - targetPose.getX()) / abLength; + + // Compute the two possible C points + double c1X = mx + d * perpX; + double c1Y = my + d * perpY; + + double c2X = mx - d * perpX; + double c2Y = my - d * perpY; + c1 = new Pose2d(c1X, c1Y, new Rotation2d(robotAngle)); + c2 = new Pose2d(c2X, c2Y, new Rotation2d(robotAngle)); + Logger.recordOutput("c1x", c1); + Logger.recordOutput("c1y", c2); + firstTimeCalculated = true; + java.util.logging.Logger.getGlobal().finer("Running once"); + } + // targetPose.getRotation().getRadians()); + // if (){ + + // } else + // if (Math.abs(yFromIntake) < 0.2) { + java.util.logging.Logger.getGlobal().finer("going in"); + // if (!firstTimeGoingInCalculated) { + // firstTimeCalculated = false; + // } + firstTimeGoingIn = true; + return targetPose; + // } else if (yFromIntake < 0.0 && !firstTimeGoingIn) { + // System.out.println("to the left"); + // return c1; + // } else { + // System.out.println("to the right"); + // return c2; + // } + // double pieceXFromIntake = noteX - intakeXOffset; + // double pieceYFromIntake = noteY - intakeYOffset; + } else + + { + // System.out.println("default game piece"); + return new Pose2d(); + } + } + + public void goToCoral() { + double yaw = 0.0; + double pitch = 0.0; + var result = peripherals.getFrontGamePieceCamResult(); + List tracks = new ArrayList<>(result.getTargets()); + List coralTargets = new ArrayList<>(); + for (PhotonTrackedTarget track : tracks) { + if (track.objDetectId == 1) { + coralTargets.add(track); + } + } + + if (result.hasTargets() && !coralTargets.isEmpty()) { + PhotonTrackedTarget bestTrack = coralTargets.get(0); + yaw = bestTrack.getYaw(); + pitch = bestTrack.getPitch(); + } + + if (yaw != 0.0 && pitch != 0.0) { + double currentAngle = -yaw; + java.util.logging.Logger.getGlobal().finer("currentAngle: " + currentAngle); + double wantedAngleInFrame = 14.7; + double cameraToRobotAngleOffset = 35.0; + rotatePID.setSetPoint(wantedAngleInFrame); + rotatePID.updatePID(currentAngle); + double r = -rotatePID.getResult(); + double driveAngleDeg = wantedAngleInFrame + cameraToRobotAngleOffset; + double wantedSpeedMPS = 1.0; + Vector v = new Vector(Math.cos(Math.toRadians(driveAngleDeg)), Math.sin(Math.toRadians(driveAngleDeg))) + .scaled(wantedSpeedMPS); + autoRobotCentricDrive(v, r); + } + } + + public Pose2d getMt2Pose2d() { + return mt2Odometry.getEstimatedPosition(); + } + + /** + * Retrieves the current X-coordinate of the robot from odometry. + * + * @return The current X-coordinate of the robot. + */ + public double getMT2OdometryX() { + return mt2Odometry.getEstimatedPosition().getX(); + } + + /** + * Retrieves the current Y-coordinate of the robot from odometry. + * + * @return The current Y-coordinate of the robot. + */ + public double getMT2OdometryY() { + return mt2Odometry.getEstimatedPosition().getY(); + } + + /** + * Retrieves the current orientation angle of the robot from odometry. + * + * @return The current orientation angle of the robot in radians. + */ + public double getMT2OdometryAngle() { + return mt2Odometry.getEstimatedPosition().getRotation().getRadians(); + } + + /** + * Drives the robot with alignment adjustment based on the specified angle from + * placement. + * + * @param degreesFromPlacement The angle in degrees from the placement + * orientation to align with. + */ + public void driveAutoAligned(double degreesFromPlacement) { + + double turn = degreesFromPlacement; + + double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double originalY = -(Math.copySign(OI.getDriverLeftX() * OI.getDriverLeftX(), OI.getDriverLeftX())); + + if (Math.abs(originalX) < 0.05) { + originalX = 0; + } + if (Math.abs(originalY) < 0.05) { + originalY = 0; + } + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + } + + /** + * Turns the robot in robot-centric mode. + * + * @param turn The rate at which the robot should turn in radians per second. + */ + public void autoRobotCentricTurn(double turn) { + frontLeft.drive(new Vector(0, 0), turn, 0.0); + frontRight.drive(new Vector(0, 0), turn, 0.0); + backLeft.drive(new Vector(0, 0), turn, 0.0); + backRight.drive(new Vector(0, 0), turn, 0.0); + } + + /** + * Drives the robot in robot-centric mode using velocity vector and turning + * rate. + * + * @param velocityVector The velocity vector containing x and y velocities in + * meters per second (m/s). + * @param turnRadiansPerSec The rate at which the robot should spin in radians + * per second. + */ + public void autoRobotCentricDrive(Vector velocityVector, double turnRadiansPerSec) { + frontLeft.drive(velocityVector, turnRadiansPerSec, 0); + frontRight.drive(velocityVector, turnRadiansPerSec, 0); + backLeft.drive(velocityVector, turnRadiansPerSec, 0); + backRight.drive(velocityVector, turnRadiansPerSec, 0); + } + + /** + * Drives the robot during teleoperation. + * + * @apiNote This method updates the fused odometry array and controls the + * robot's movement based on joystick inputs. + */ + public void teleopDrive() { + double oiRX = OI.getDriverRightX(); + double oiLX = OI.getDriverLeftX(); + double oiRY = OI.getDriverRightY(); + double oiLY = OI.getDriverLeftY(); + if (OI.operatorLT.getAsBoolean() && OI.operatorRT.getAsBoolean()) { + oiRX = OI.getOperatorRightX(); + oiLX = OI.getOperatorLeftX(); + oiRY = OI.getOperatorRightY(); + oiLY = OI.getOperatorLeftY(); + } + + double turnLimit = 0.17; + + if (OI.driverController.getRightTriggerAxis() > 0.2 || OI.getDriverRB()) { + // activate slowy spin + turnLimit = 0.1; + oiRX = oiRX * 0.8; + oiLX = oiLX * 0.8; + oiRY = oiRY * 0.8; + oiLY = oiLY * 0.8; + } + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + double turn = turnLimit + * (oiRX * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.05) { + turn = 0.0; + } + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + // } + } + + public void robotCentricDrive(double angle) { + double oiRX = OI.getDriverRightX(); + double oiLX = OI.getDriverLeftX(); + double oiRY = OI.getDriverRightY(); + double oiLY = OI.getDriverLeftY(); + + // Logger.recordOutput("Adjusted Right X", oiRX); + // Logger.recordOutput("Adjusted Left X", oiLX); + // Logger.recordOutput("Adjusted Right Y", oiRY); + // Logger.recordOutput("Adjusted Left Y", oiLY); + + double turnLimit = 0.17; + // 0.35 before + + if (OI.driverController.getRightTriggerAxis() > 0.2) { + // activate slowy spin + turnLimit = 0.1; + oiRX = oiRX * 0.5; + oiLX = oiLX * 0.5; + oiRY = oiRY * 0.5; + oiLY = oiLY * 0.5; + } + + // this is correct, X is forward in field, so originalX should be the y on the + // // joystick + // double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), + // OI.getDriverLeftY())); + // double originalY = -(Math.copySign(OI.getDriverLeftX() * OI.getDriverLeftX(), + // OI.getDriverLeftX())); + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + // if (Math.abs(originalX) < 0.005) { + // originalX = 0; + // } + // if (Math.abs(originalY) < 0.005) { + // originalY = 0; + // } + + // double turn = turnLimit * ((Math.copySign(OI.getDriverRightX() * + // OI.getDriverRightX() * OI.getDriverRightX(), OI.getDriverRightX())) * + // (Constants.Physical.TOP_SPEED)/(Constants.Physical.ROBOT_RADIUS)); + double turn = turnLimit + * (oiRX * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.05) { + turn = 0.0; + } + + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, Math.toRadians(angle)); + frontRight.drive(controllerVector, turn, Math.toRadians(angle)); + backLeft.drive(controllerVector, turn, Math.toRadians(angle)); + backRight.drive(controllerVector, turn, Math.toRadians(angle)); + // } + } + + public void teleopDriveToPiece(double yToPiece) { + double turnLimit = 0.17; + double kP = 0.8; + // 0.35 before + + // if (OI.driverController.getLeftBumper()) { + // // activate speedy spin + // // turnLimit = 1; //TODO: find a different keybind for this + // } + + // this is correct, X is forward in field, so originalX should be the y on the + // joystick + double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double originalY = yToPiece * kP; + + if (Math.abs(originalX) < 0.075) { + originalX = 0; + } + + double turn = turnLimit + * (OI.getDriverRightX() * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.15) { + turn = 0.0; + } + + if (turn == 0.0) { + turningPID.setSetPoint(angleSetpoint); + double yaw = peripherals.getPigeonAngle(); + + yaw = Constants.standardizeAngleToOtherDegrees(yaw, angleSetpoint); + double result = -2 * turningPID.updatePID(yaw); + // Logger.recordOutput("result", result); + + double x = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double y = yToPiece * kP; + + if (Math.abs(originalX) < 0.05) { + originalX = 0; + } + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(x, y); + double yPower = getAdjustedY(x, y); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, result, pigeonAngle); + frontRight.drive(controllerVector, result, pigeonAngle); + backLeft.drive(controllerVector, result, pigeonAngle); + backRight.drive(controllerVector, result, pigeonAngle); + } else { + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + } + } + + private int hitNumber = 0; + private int hitNumberSemiGenerous = 0; + private int hitNumberGenerous = 0; + private int hitNumberUltraGenerous = 0; + + public boolean hitSetPoint(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.045 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 1.5) { + hitNumber += 1; + } else { + hitNumber = 0; + } + if (hitNumber > 1) { + return true; + } else { + return false; + } + } + + public boolean hitSetPointSemiGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + Logger.recordOutput("Error for semi-generous", Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2))); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.05 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 2) { + hitNumberSemiGenerous += 1; + } else { + hitNumberSemiGenerous = 0; + } + if (hitNumberSemiGenerous > 3) { + return true; + } else { + return false; + } + } + + public Pose2d getReefClosestSetpointFrontOnly(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositions.get(i).getX() + + Constants.Reef.redBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositions.get(i).getY() + + Constants.Reef.redBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositions.get(i).getX() + + Constants.Reef.blueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositions.get(i).getY() + + Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public boolean hitSetPointGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.10 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 2.5) { + hitNumberGenerous += 1; + } else { + hitNumberGenerous = 0; + } + if (hitNumberGenerous > 3) { + return true; + } else { + return false; + } + } + + public boolean hitSetPointUltraGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.10 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 10.0) { + hitNumberUltraGenerous += 1; + } else { + hitNumberUltraGenerous = 0; + } + if (hitNumberUltraGenerous > 2) { + return true; + } else { + return false; + } + } + + public void driveToPoint(Pose2d targetPoint) { + Logger.recordOutput("Goal X, Y, Theta", targetPoint); + // Logger.recordOutput("Magnitude Error Inches", + // Constants.metersToInches(Math.sqrt(Math.pow(x - getMT2OdometryX(), 2) + + // Math.pow(y - getMT2OdometryY(), 2)))); + // Logger.recordOutput("Theta Error Degrees", Math.toDegrees(theta - + // getMT2OdometryAngle())); + double x = targetPoint.getX(); + double y = targetPoint.getY(); + double theta = targetPoint.getRotation().getRadians(); + theta = Constants.standardizeAngleToOther(theta, getMT2OdometryAngle()); + + double xVelNoFF = 0.0; + double yVelNoFF = 0.0; + double thetaVelNoFF = 0.0; + + if (DriverStation.isAutonomousEnabled() && systemState.equals(DriveState.L4_REEF)) { + xxPID4A.setSetPoint(x); + yyPID4A.setSetPoint(y); + thetaaPID4A.setSetPoint(theta); + + xxPID4A.updatePID(getMT2OdometryX()); + yyPID4A.updatePID(getMT2OdometryY()); + thetaaPID4A.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID4A.getResult(); + yVelNoFF = yyPID4A.getResult(); + // double velmag = Math.hypot(xVelNoFF, yVelNoFF); + // double maxvel = 0.1; + // if (velmag > maxvel) { + // xVelNoFF = xVelNoFF * maxvel / velmag; + // yVelNoFF = yVelNoFF * maxvel / velmag; + // } + thetaVelNoFF = -thetaaPID4A.getResult(); + } else if (OI.driverPOVRight.getAsBoolean()) { + xxPID4.setSetPoint(x); + yyPID4.setSetPoint(y); + thetaaPID4.setSetPoint(theta); + + xxPID4.updatePID(getMT2OdometryX()); + yyPID4.updatePID(getMT2OdometryY()); + thetaaPID4.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID4.getResult(); + yVelNoFF = yyPID4.getResult(); + thetaVelNoFF = -thetaaPID4.getResult(); + + } else if (DriverStation.isTeleopEnabled() + && (OI.driverPOVLeft.getAsBoolean() || OI.driverPOVDown.getAsBoolean())) { + + xxPID23.setSetPoint(x); + yyPID23.setSetPoint(y); + thetaaPID23.setSetPoint(theta); + + xxPID23.updatePID(getMT2OdometryX()); + yyPID23.updatePID(getMT2OdometryY()); + thetaaPID23.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID23.getResult(); + yVelNoFF = yyPID23.getResult(); + thetaVelNoFF = -thetaaPID23.getResult(); + + } else if (DriverStation.isTeleopEnabled() && OI.driverPOVUp.getAsBoolean()) { + + xxPID1.setSetPoint(x); + yyPID1.setSetPoint(y); + thetaaPID1.setSetPoint(theta); + + xxPID1.updatePID(getMT2OdometryX()); + yyPID1.updatePID(getMT2OdometryY()); + thetaaPID1.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID1.getResult(); + yVelNoFF = yyPID1.getResult(); + thetaVelNoFF = -thetaaPID1.getResult(); + + } else if (systemState.equals(DriveState.PIECE_PICKUP)) { + + xxPIDPickup.setSetPoint(x); + yyPIDPickup.setSetPoint(y); + thetaaPIDPickup.setSetPoint(theta); + + xxPIDPickup.updatePID(getMT2OdometryX()); + yyPIDPickup.updatePID(getMT2OdometryY()); + thetaaPIDPickup.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPIDPickup.getResult(); + yVelNoFF = yyPIDPickup.getResult(); + thetaVelNoFF = -thetaaPIDPickup.getResult(); + + } else { + + xxPID.setSetPoint(x); + yyPID.setSetPoint(y); + thetaaPID.setSetPoint(theta); + + xxPID.updatePID(getMT2OdometryX()); + yyPID.updatePID(getMT2OdometryY()); + thetaaPID.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID.getResult(); + yVelNoFF = yyPID.getResult(); + thetaVelNoFF = -thetaaPID.getResult(); + } + + // double feedForwardX = targetPoint.getDouble("x_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + // double feedForwardY = targetPoint.getDouble("y_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + // double feedForwardTheta = -targetPoint.getDouble("angular_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + + double finalX = xVelNoFF; + double finalY = yVelNoFF; + double finalTheta = thetaVelNoFF; + // if (m_fieldSide == "blue") { + // finalX = -finalX; + // finalTheta = -finalTheta; + // } + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + }; + + Vector velocityVector = new Vector(); + double desiredThetaChange = 0; + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(velocityArray[1].doubleValue()); + desiredThetaChange = velocityArray[2].doubleValue(); + + autoDrive(velocityVector, desiredThetaChange); + + } + + public void orbitDrive(Pose2d end, Pose2d pre, Pose2d current) { + // end: C, pre: B, current: A + Translation2d p = end.minus(pre).getTranslation(); // a vector in direction BC + Translation2d d = pre.minus(current).getTranslation(); // a vector in direction AB + Rotation2d alpha = p.getAngle().minus(d.getAngle()); // find angle between BC and AB + Rotation2d direction = d.getAngle().minus(alpha); + double velocity = getSpeedUsingPhysics(end.minus(current).getTranslation().getNorm(), 0); + double xVel = velocity * Math.cos(direction.getRadians()); + double yVel = velocity * Math.sin(direction.getRadians()); + Vector velocityVector = new Vector(xVel, yVel); + thetaaPID4.setSetPoint(end.getRotation().getRadians()); + double desiredThetaChange = thetaaPID4.getResult(); // TODO: Test if this PID is good. + autoDrive(velocityVector, desiredThetaChange); + } + + public double getSpeedUsingPhysics(double distance, double finalVel) { + // Max Velocity at which the robot can slow down given the Max Deceleration + // (-Constants.Physical.MAX_ACCELERATION) + return Math.sqrt(finalVel * finalVel - (2 * (-Constants.Physical.MAX_ACCELERATION) * distance)); + } + + public double clampToForwardAccelerationLimit(double currentVelocity, double wantedAcceleration) { + return Math.min(wantedAcceleration, + Constants.Physical.MAX_ACCELERATION * (1 - (currentVelocity / Constants.Physical.TOP_SPEED))); + } + + public void driveToXTheta(double x, double theta) { + java.util.logging.Logger.getGlobal().finer(theta + ""); + // theta = Math.toRadians(theta); + theta = Constants.standardizeAngleToOther(theta, getMT2OdometryAngle()); + xxPID.setSetPoint(x); + thetaaPID.setSetPoint(theta); + + xxPID.updatePID(getMT2OdometryX()); + thetaaPID.updatePID(getMT2OdometryAngle()); + + double xVelNoFF = xxPID.getResult(); + double yVelNoFF = OI.getDriverLeftX() * 2.9; + double thetaVelNoFF = -thetaaPID.getResult(); + double finalX = xVelNoFF; + double finalY = yVelNoFF; + double finalTheta = thetaVelNoFF; + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + }; + + Vector velocityVector = new Vector(); + double desiredThetaChange = 0; + if (getFieldSide().equals("red")) { + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(-velocityArray[1].doubleValue()); + } else { + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(velocityArray[1].doubleValue()); + } + desiredThetaChange = velocityArray[2].doubleValue(); + + autoDrive(velocityVector, desiredThetaChange); + } + + public void driveOnLine(Vector lineVector, Translation2d pointOnLine, double angrad) { + Logger.recordOutput("Point On Line", new Pose2d(pointOnLine, new Rotation2d())); + Logger.recordOutput("LineVector", + new Pose2d(pointOnLine.plus(new Translation2d(lineVector.getI(), lineVector.getJ())), + new Rotation2d())); + double oiLY = OI.getDriverLeftY(); + double oiLX = OI.getDriverLeftX(); + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-ySpeed); + controllerVector.setJ(xSpeed); + } + Vector projectedJoystick = lineVector.projectOther(controllerVector); + Logger.recordOutput("controllerProjection", + new Pose2d(new Translation2d(projectedJoystick.getI() + getMT2OdometryX(), + projectedJoystick.getJ() + getMT2OdometryY()), new Rotation2d(angrad))); + Translation2d currentPoint = new Translation2d(getMT2OdometryX(), getMT2OdometryY()); + Translation2d closestPointOnLine = lineVector.getClosestPointOnLine(pointOnLine, currentPoint); + Logger.recordOutput("closestPointOnLine", new Pose2d(closestPointOnLine, new Rotation2d(angrad))); + xxPID.setSetPoint(closestPointOnLine.getX()); + yyPID.setSetPoint(closestPointOnLine.getY()); + angrad = Constants.standardizeAngleToOther(angrad, getMT2OdometryAngle()); + thetaaPID.setSetPoint(angrad); + double toPointXVel = xxPID.updatePID(getMT2OdometryX()); + double toPointYVel = -yyPID.updatePID(getMT2OdometryY()); + double thetaVel = -thetaaPID.updatePID(getMT2OdometryAngle()); + Logger.recordOutput("feederAngle", Math.toDegrees(angrad)); + Logger.recordOutput("thetaVel", thetaVel); + Logger.recordOutput("toPointYVel", toPointYVel); + Logger.recordOutput("toPointXVel", toPointXVel); + Vector toPointVector = new Vector(toPointXVel, toPointYVel); + Vector driveVector = toPointVector.add(projectedJoystick); + Logger.recordOutput("driveVector", new Pose2d(new Translation2d(driveVector.getI() + getMT2OdometryX(), + driveVector.getJ() + getMT2OdometryY()), new Rotation2d(angrad))); + autoDrive(driveVector, thetaVel); + } + + public void driveToTheta(double theta) { + theta = Constants.standardizeAngleToOtherDegrees(theta, getMT2OdometryAngle()); + + // Logger.recordOutput("Drive Angle Setpoint", theta); + turningPID.setSetPoint(theta); + turningPID.updatePID(Math.toDegrees(getMT2OdometryAngle())); + + double result = -turningPID.getResult(); + if (Math.abs(Math.toDegrees(getMT2OdometryAngle()) - theta) < 2) { + result = 0; + } + driveAutoAligned(result); + } + + /** + * Runs autonomous driving by providing velocity vector and turning rate. + * + * @param vector The velocity vector containing xy velocities. + * @param turnRadiansPerSec The rate at which the robot should spin in radians + * per second. + */ + public void autoDrive(Vector vector, double turnRadiansPerSec) { + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + + frontLeft.drive(vector, turnRadiansPerSec, pigeonAngle); + frontRight.drive(vector, turnRadiansPerSec, pigeonAngle); + backLeft.drive(vector, turnRadiansPerSec, pigeonAngle); + backRight.drive(vector, turnRadiansPerSec, pigeonAngle); + } + + /** + * Retrieves the current velocity vector of the robot in field coordinates. + * The velocity vector is calculated based on the individual wheel speeds and + * orientations. + * + * @return The current velocity vector of the robot in meters per second (m/s). + */ + public Vector getRobotVelocityVector() { + Vector velocityVector = new Vector(0, 0); + double pigeonAngleRadians = Math.toRadians(this.peripherals.getPigeonAngle()); + + double frV = this.frontRight.getGroundSpeed(); + double frTheta = this.frontRight.getWheelPosition() + pigeonAngleRadians; + double frVX = frV * Math.cos(frTheta); + double frVY = frV * Math.sin(frTheta); + double flV = this.frontLeft.getGroundSpeed(); + double flTheta = this.frontLeft.getWheelPosition() + pigeonAngleRadians; + double flVX = flV * Math.cos(flTheta); + double flVY = flV * Math.sin(flTheta); + double blV = this.backLeft.getGroundSpeed(); + double blTheta = this.backLeft.getWheelPosition() + pigeonAngleRadians; + double blVX = blV * Math.cos(blTheta); + double blVY = blV * Math.sin(blTheta); + double brV = this.backRight.getGroundSpeed(); + double brTheta = this.backRight.getWheelPosition() + pigeonAngleRadians; + double brVX = brV * Math.cos(brTheta); + double brVY = brV * Math.sin(brTheta); + + velocityVector.setI(frVX + flVX + blVX + brVX); + velocityVector.setJ(frVY + flVY + blVY + brVY); + + // System.out.println("VVector: <" + velocityVector.getI() + ", " + + // velocityVector.getJ() + ">"); + + return velocityVector; + } + + /** + * Retrieves the acceleration vector of the robot. + * The acceleration is calculated based on the linear acceleration measured by + * the Pigeon IMU. + * The acceleration is expressed in field-centric coordinates. + * + * @return The acceleration vector of the robot in meters per second squared + * (m/s^2). + */ + public Vector getRobotAccelerationVector() { + Vector accelerationVector = new Vector(); + + Vector robotCentricAccelerationVector = this.peripherals.getPigeonLinAccel(); + double accelerationMagnitude = Constants.getDistance(robotCentricAccelerationVector.getI(), + robotCentricAccelerationVector.getJ(), 0, 0); + accelerationVector.setI(accelerationMagnitude * Math.cos(Math.toRadians(this.peripherals.getPigeonAngle()))); + accelerationVector.setJ(accelerationMagnitude * Math.sin(Math.toRadians(this.peripherals.getPigeonAngle()))); + + return accelerationVector; + } + + /** + * Retrieves the path point closest to the specified time from the given path. + * If the specified time is before the first path point, the first point is + * returned. + * If the specified time is after the last path point, the last point is + * returned. + * + * @param path The array containing path points, each represented as a + * JSONArray. + * @param time The time for which the closest path point is required. + * @return The closest path point to the specified time. + */ + public JSONArray getPathPoint(JSONArray path, double time) { + for (int i = 0; i < path.length() - 1; i++) { + JSONArray currentPoint = path.getJSONArray(i + 1); + JSONArray previousPoint = path.getJSONArray(i); + double currentPointTime = currentPoint.getDouble(0); + double previousPointTime = previousPoint.getDouble(0); + if (time >= previousPointTime && time < currentPointTime) { + return currentPoint; + } + } + if (time < path.getJSONArray(0).getDouble(0)) { + return path.getJSONArray(0); + } else { + return path.getJSONArray(path.length() - 1); + } + } + + public Number[] purePursuitController(double currentX, double currentY, double currentTheta, int currentIndex, + JSONArray pathPoints, boolean fullSend, boolean accurate) { + JSONObject targetPoint = pathPoints.getJSONObject(pathPoints.length() - 1); + int targetIndex = pathPoints.length() - 1; + if (this.m_fieldSide == "blue") { + currentX = Constants.Physical.FIELD_LENGTH - currentX; + currentY = Constants.Physical.FIELD_WIDTH - currentY; + currentTheta = Math.PI + currentTheta; + } + + for (int i = currentIndex; i < pathPoints.length(); i++) { + JSONObject point = pathPoints.getJSONObject(i); + double targetX = point.getDouble("x"), targetY = point.getDouble("y"), + targetTheta = point.getDouble("angle"), targetXvel = point.getDouble("x_velocity"), + targetYvel = point.getDouble("y_velocity"), targetThetavel = point.getDouble("angular_velocity"); + targetTheta = Constants.standardizeAngleToOther(targetTheta, currentTheta); + double linearVelMag = Math.hypot(targetYvel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + targetXvel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS); + double targetVelMag = Math.hypot(linearVelMag, + targetThetavel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS); + double lookaheadRadius = fullSend ? Constants.Autonomous.FULL_SEND_LOOKAHEAD + : Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_DISTANCE * targetVelMag + + Constants.Autonomous.MIN_LOOKAHEAD_DISTANCE;// If full send mode is enabled, use the full + // send lookahead + double deltaX = (currentX - targetX), deltaY = (currentY - targetY), + deltaTheta = (currentTheta - targetTheta); + if (!insideRadius(deltaX / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + deltaY / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + deltaTheta / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS, + lookaheadRadius)) { + targetIndex = i; + targetPoint = pathPoints.getJSONObject(i); + break; + } + } + double targetX = targetPoint.getDouble("x"), targetY = targetPoint.getDouble("y"), + targetTheta = targetPoint.getDouble("angle"); + + targetTheta = Constants.standardizeAngleToOther(targetTheta, currentTheta); + + xPID.setSetPoint(targetX); + yPID.setSetPoint(targetY); + thetaPID.setSetPoint(targetTheta); + + xPID.updatePID(currentX); + yPID.updatePID(currentY); + thetaPID.updatePID(currentTheta); + double pidScaler = 1; + double xVelNoFF = xPID.getResult() * pidScaler; + double yVelNoFF = yPID.getResult() * pidScaler; + double thetaVelNoFF = -thetaPID.getResult(); + double f = (accurate ? Constants.Autonomous.ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY + : Constants.Autonomous.FEED_FORWARD_MULTIPLIER); + double feedForwardX = targetPoint.getDouble("x_velocity") * f; + double feedForwardY = targetPoint.getDouble("y_velocity") * f; + double feedForwardTheta = -targetPoint.getDouble("angular_velocity") * f * 0.1; + + double finalX = xVelNoFF + feedForwardX; + double finalY = yVelNoFF + feedForwardY; + double finalTheta = (thetaVelNoFF + feedForwardTheta) * 1.25; + if (m_fieldSide == "blue") { + finalX = -finalX; + finalY = -finalY; + } + + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + targetIndex, + }; + double linearVelMag = Math.hypot( + targetPoint.getDouble("x_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + targetPoint.getDouble("y_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS); + double targetVelMag = Math.hypot(linearVelMag, + targetPoint.getDouble("angular_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS); + double lookaheadRadius = fullSend ? Constants.Autonomous.FULL_SEND_LOOKAHEAD + : Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_DISTANCE * targetVelMag + + Constants.Autonomous.MIN_LOOKAHEAD_DISTANCE; + + Logger.recordOutput("x-vel", xVelNoFF); + Logger.recordOutput("y-vel", yVelNoFF); + Logger.recordOutput("theta-vel", thetaVelNoFF); + Logger.recordOutput("wanted-theta-vel", + targetPoint.getDouble("angular_velocity")); + Logger.recordOutput("FF-theta-vel", feedForwardTheta); + Logger.recordOutput("FF-x-vel", feedForwardX); + Logger.recordOutput("FF-y-vel", feedForwardY); + Logger.recordOutput("current point idx", currentIndex); + Logger.recordOutput("point idx", velocityArray[3].intValue()); + Logger.recordOutput("look-ahead", lookaheadRadius); + Logger.recordOutput("target-point", new Pose2d(targetX, targetY, new Rotation2d(targetTheta))); + Logger.recordOutput("Velocity Array", + new double[] { finalX, -finalY, finalTheta }); + Logger.recordOutput("dx", targetX - currentX); + Logger.recordOutput("dy", targetY - currentY); + Logger.recordOutput("dtheta", targetTheta - currentTheta); + return velocityArray; + } + + public boolean insideRadius(double deltaX, double deltaY, double deltaTheta, double radius) { + // Logger.recordOutput("deltax", deltaX); + // Logger.recordOutput("deltay", deltaY); + // Logger.recordOutput("deltaTheta", deltaTheta); + Logger.recordOutput("Error", Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2) + Math.pow(deltaTheta, 2))); + return Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2) + Math.pow(deltaTheta, 2)) < radius; + } + + public void calculateAngleChange(double angle) { + double pigeonAngleDegrees = this.peripherals.getPigeonAngle(); + double targetAngle = 0; + if (getFieldSide() == "red") { + targetAngle = angle + 180; + } else { + targetAngle = angle; + } + + if (DriverStation.isAutonomousEnabled() && getFieldSide() == "red") { + pigeonAngleDegrees = 180 + pigeonAngleDegrees; + } + this.turningPID.setSetPoint(Constants.standardizeAngleDegrees(targetAngle)); + this.turningPID.updatePID(Constants.standardizeAngleDegrees(pigeonAngleDegrees)); + double turnResult = -turningPID.getResult(); + + this.driveAutoAligned(turnResult); + } + + private DriveState handleStateTransition() { + switch (wantedState) { + case DEFAULT: + return DriveState.DEFAULT; + case IDLE: + return DriveState.IDLE; + case REEF: + return DriveState.REEF; + case REEF_MORE: + return DriveState.REEF_MORE; + case BACK: + return DriveState.BACK; + case L3_REEF: + return DriveState.L3_REEF; + case L4_REEF: + return DriveState.L4_REEF; + case ALGAE: + return DriveState.ALGAE; + case ALGAE_MORE: + return DriveState.ALGAE_MORE; + case ALGAE_MORE_MORE: + return DriveState.ALGAE_MORE_MORE; + case PROCESSOR: + return DriveState.PROCESSOR; + case PROCESSOR_MORE: + return DriveState.PROCESSOR_MORE; + case NET: + return DriveState.NET; + case NET_MORE: + return DriveState.NET_MORE; + case FEEDER: + return DriveState.FEEDER; + case SCORE_L23: + return DriveState.SCORE_L23; + case AUTO_FEEDER: + return DriveState.AUTO_FEEDER; + case FEEDER_ALIGN: + return DriveState.FEEDER_ALIGN; + case AUTO_L1: + return DriveState.AUTO_L1; + case AUTO_L1_MORE: + return DriveState.AUTO_L1_MORE; + case FEEDER_AUTO: + return DriveState.FEEDER_AUTO; + case PIECE_PICKUP: + return DriveState.PIECE_PICKUP; + case AUTO_CLIMB: + return DriveState.AUTO_CLIMB; + case STOP: + return DriveState.STOP; + default: + return DriveState.IDLE; + } + } + + Vector scoreL23Vector = new Vector(2.5, 0); + Pose2d l23Setpoint = new Pose2d(); + + Vector pickupAlgaeFrontVector = new Vector(2.5, 0); + Pose2d algaeSetpoint = new Pose2d(); + Vector pickupAlgaeBackVector = new Vector(-2.5, 0); + + public double getDistanceFromL23Setpoint() { + return l23Setpoint.getTranslation().getDistance(getMT2Odometry().getTranslation()); + } + + public double getDistanceFromAlgaeSetpoint() { + return algaeSetpoint.getTranslation().getDistance(getMT2Odometry().getTranslation()); + } + + public double getThetaToCenterReef() { + double theta = 0.0; + { + theta = Math.atan2((Constants.Reef.centerBlue.getY() - getMT2OdometryY()), + (Constants.Reef.centerBlue.getX() - getMT2OdometryX())); + } + if (getAngleDifferenceDegrees(Math.toDegrees(theta), Math.toDegrees(getMT2OdometryAngle())) > 90.0) { + theta -= Math.PI; + } + return theta; + } + + public double[] getClosestPoint(double[] lineStart, double[] lineEnd) { // chatGPT ahh code + // System.out.println("X1 - X2; " + (lineStart[0] - lineEnd[0]) + "Y1 - Y2" + + // (lineStart[1] - lineEnd[1])); + double x1 = lineStart[0]; + double y1 = lineStart[1]; + double x2 = lineEnd[0]; + double y2 = lineEnd[1]; + double px = getMT2OdometryX(); + double py = getMT2OdometryY(); + + double dx = x2 - x1; + double dy = y2 - y1; + double lenSq = dx * dx + dy * dy; + + if (lenSq == 0) { + return new double[] { x1, y1 }; + } + + double t = ((px - x1) * dx + (py - y1) * dy) / lenSq; + t = Math.max(0, Math.min(1, t)); + + double closestX = x1 + t * dx; + double closestY = y1 + t * dy; + + return new double[] { closestX, closestY }; + } + + public double getThetaToPoint(double xMeters, double yMeters) { + return Math.atan2(yMeters - getMT2OdometryY(), + xMeters - getMT2OdometryX()); + } + + public double[] getClosestL1PointXY() { // returns the closest point on the trough for l1 + // double[] xy = { 0.0, 0.0 }; + // if (isL1Face()) { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(1)); + // } else { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(0)); + // } + // return xy; + // System.out.println("X1: " + getClosestL1Points().get(0)[0] + " Y1: " + + // getClosestL1Points().get(0)[1] + "X2: " + // + getClosestL1Points().get(1)[0] + " Y2: " + getClosestL1Points().get(1)[1]); + double[] xy = getClosestPoint(getClosestL1Points().get(0), getClosestL1Points().get(1)); + return xy; + } + + public double[] getClosestL1CornerPointXY() { // returns the closest point on the trough for l1 + // double[] xy = { 0.0, 0.0 }; + // if (isL1Face()) { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(1)); + // } else { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(0)); + // } + // return xy; + // System.out.println("X1: " + getClosestL1Points().get(0)[0] + " Y1: " + + // getClosestL1Points().get(0)[1] + "X2: " + // + getClosestL1Points().get(1)[0] + " Y2: " + getClosestL1Points().get(1)[1]); + double[] xy = getClosestPoint(getClosestL1CornerPoints().get(0), getClosestL1CornerPoints().get(1)); + return xy; + } + + public ArrayList getClosestL1Points() { + ArrayList list = new ArrayList<>(); + double currentDist = 100.0; + double dist = 100.0; + double[] chosenSetpoint = { 0.0, 0.0 }; + double[] secondPoint = { 0.0, 0.0 }; + if (isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueDrivePoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1BlueDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1BlueDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + // System.out.println("Closest Point: " + list.get(0)[0] + " " + + // list.get(0)[1]); + currentDist = 100.0; + dist = 100.0; + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueDrivePoints.get(i).getY())); + if (currentDist < dist + && Math.hypot(Math.abs(Constants.Reef.l1BlueDrivePoints.get(i).getX() - list.get(0)[0]), + Math.abs(Constants.Reef.l1BlueDrivePoints.get(i).getY() - list.get(0)[1])) > 0.1) { + dist = currentDist; + // System.out.println(" "); + // System.out.println("new X2: " + Constants.Reef.l1BluePoints.get(i).getX() + + // "new Y2: " + // + Constants.Reef.l1BluePoints.get(i).getY()); + // System.out.println("old X2: " + list.get(0)[0] + "old Y2: " + // + list.get(0)[1]); + secondPoint[0] = Constants.Reef.l1BlueDrivePoints.get(i).getX(); + secondPoint[1] = Constants.Reef.l1BlueDrivePoints.get(i).getY(); + } + } + // System.out.println("->->->->->->->->->"); + list.add(secondPoint); + return list; + } else { + for (int i = 0; i < Constants.Reef.l1RedDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedDrivePoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + currentDist = 100.0; + dist = 100.0; + chosenSetpoint[0] = 0.0; + chosenSetpoint[1] = 0.0; + for (int i = 0; i < Constants.Reef.l1RedDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedDrivePoints.get(i).getY())); + if (currentDist < dist && Math.abs(Constants.Reef.l1RedDrivePoints.get(i).getX() - list.get(0)[0]) > 0.1 + && Math.abs(Constants.Reef.l1RedDrivePoints.get(i).getY() - list.get(0)[1]) > 0.1) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + return list; + + } + } + + public ArrayList getClosestL1CornerPoints() { + ArrayList list = new ArrayList<>(); + double currentDist = 100.0; + double dist = 100.0; + double[] chosenSetpoint = { 0.0, 0.0 }; + double[] secondPoint = { 0.0, 0.0 }; + if (isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.l1BlueCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueCornerPoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1BlueCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1BlueCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + // System.out.println("Closest Point: " + list.get(0)[0] + " " + + // list.get(0)[1]); + currentDist = 100.0; + dist = 100.0; + for (int i = 0; i < Constants.Reef.l1BlueCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueCornerPoints.get(i).getY())); + if (currentDist < dist + && Math.hypot(Math.abs(Constants.Reef.l1BlueCornerPoints.get(i).getX() - list.get(0)[0]), + Math.abs(Constants.Reef.l1BlueCornerPoints.get(i).getY() - list.get(0)[1])) > 0.1) { + dist = currentDist; + // System.out.println(" "); + // System.out.println("new X2: " + + // Constants.Reef.l1BlueCornerPoints.get(i).getX() + "new Y2: " + // + Constants.Reef.l1BlueCornerPoints.get(i).getY()); + // System.out.println("old X2: " + list.get(0)[0] + "old Y2: " + // + list.get(0)[1]); + secondPoint[0] = Constants.Reef.l1BlueCornerPoints.get(i).getX(); + secondPoint[1] = Constants.Reef.l1BlueCornerPoints.get(i).getY(); + } + } + // System.out.println("->->->->->->->->->"); + list.add(secondPoint); + return list; + } else { + for (int i = 0; i < Constants.Reef.l1RedCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedCornerPoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + currentDist = 100.0; + dist = 100.0; + chosenSetpoint[0] = 0.0; + chosenSetpoint[1] = 0.0; + for (int i = 0; i < Constants.Reef.l1RedCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedCornerPoints.get(i).getY())); + if (currentDist < dist + && Math.abs(Constants.Reef.l1RedCornerPoints.get(i).getX() - list.get(0)[0]) > 0.1 + && Math.abs(Constants.Reef.l1RedCornerPoints.get(i).getY() - list.get(0)[1]) > 0.1) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + return list; + + } + } + + public boolean isL1Face() { // returns true if the robot is by a face of the L1, returns false if the robot + // is around the point on the edge of l1 faces + if (isOnBlueSide()) { + + } else { + + } + return true; // TODO: theoretically this should work because the closest + } + + public Pose2d getClosestPose(Pose2d pose1, Pose2d pose2) { + double dist1 = Math.hypot(Math.abs(pose1.getX() - getMT2OdometryX()), + Math.abs(pose1.getY() - getMT2OdometryY())); + double dist2 = Math.hypot(Math.abs(pose2.getX() - getMT2OdometryX()), + Math.abs(pose2.getY() - getMT2OdometryY())); + if (dist1 <= dist2) { + return pose1; + } else { + return pose2; + } + } + + public Pose2d origionalSetpointPose = new Pose2d(); + public boolean firstTimeReef = true; + + public double getNetXSetpoint() { + if (isOnBlueSide()) { + return Constants.Reef.netBlueXM; + } else { + return Constants.Reef.netRedXM; + } + } + + public double getNetXMoreSetpoint() { + if (isOnBlueSide()) { + return Constants.Reef.netBlueXMore; + } else { + return Constants.Reef.netRedXMore; + } + } + + public double getNetThetaSetpoint() { + if (isOnBlueSide()) { + if (autoPlacingFront) { + return Constants.Reef.netBlueFrontThetaR; + } else { + return Constants.Reef.netBlueBackThetaR; + } + } else { + if (autoPlacingFront) { + return Constants.Reef.netRedFrontThetaR; + } else { + return Constants.Reef.netRedBackThetaR; + } + } + } + + public boolean isOnBlueSide() { + if (getMT2OdometryX() < Constants.Physical.FIELD_LENGTH / 2.0) { + return true; + } else { + return false; + } + } + + public double[] getNetXTheta() { + double goalX = getMT2OdometryX(); + double goalTheta = getMT2OdometryAngle(); + + if (isOnBlueSide()) { + goalX = Constants.Reef.netBlueXM; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netBlueFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netBlueBackThetaR; + } + } else { + goalX = Constants.Reef.netRedXM; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netRedFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netRedBackThetaR; + } + } + + double[] xTheta = { goalX, goalTheta }; + return xTheta; + } + + public double[] getNetMoreXTheta() { + double goalX = getMT2OdometryX(); + double goalTheta = getMT2OdometryAngle(); + + if (isOnBlueSide()) { + goalX = Constants.Reef.netBlueXMore; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netBlueFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netBlueBackThetaR; + } + } else { + goalX = Constants.Reef.netRedXMore; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netRedFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netRedBackThetaR; + } + } + + double[] xTheta = { goalX, goalTheta }; + return xTheta; + } + + public double distanceFromCenterOfReef() { + if (isOnBlueSide()) { + return Math.hypot((getMT2OdometryX() - Constants.Reef.centerBlue.getX()), + ((getMT2OdometryY() - Constants.Reef.centerBlue.getY()))); + } else { + return Math.hypot((getMT2OdometryX() - Constants.Reef.centerRed.getX()), + ((getMT2OdometryY() - Constants.Reef.centerRed.getY()))); + } + } + + private final Vector backupVector = new Vector(-20.0, 0.0); + private final Vector otherBackupVector = new Vector(20.0, 0.0); + + @Override + public void periodic() { + Logger.recordOutput("Extra Pigeon Angle", peripherals.getPigeonExtraAngle()); + Logger.recordOutput("Robot Velocity", getRobotSpeed()); + Logger.recordOutput("MT2 Odometry", getMT2Odometry()); + // Pose2d target = getGamePiecePosition(); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + // Translation2d t1 = new Translation2d(getMT2OdometryX(), getMT2OdometryY()); + // Rotation2d r1 = new Rotation2d(getThetaToCenterReef()); + // Pose2d p1 = new Pose2d(t1, r1); + // Logger.recordOutput("L1 Auto Angle", p1); + l23Setpoint = getReefClosestSetpoint(getMT2Odometry(), false); + algaeSetpoint = getAlgaeClosestSetpoint(getMT2Odometry()); + // Logger.recordOutput("Robot Odometry", getMT2Odometry()); + updateOdometryFusedArray(); + // process inputs + DriveState newState = handleStateTransition(); + Pose2d setpoint = new Pose2d(); + double standardizedAngle = Constants.standardizeAngleDegrees(Math.toDegrees(getMT2OdometryAngle())); + if (newState != systemState) { + systemState = newState; + } + Logger.recordOutput("Drive State", systemState); + // Stop moving when disabled + if (DriverStation.isDisabled()) { + systemState = DriveState.DEFAULT; + } + + if (!systemState.equals(DriveState.AUTO_CLIMB)) { + firstClimb = false; + } + if (!systemState.equals(DriveState.PIECE_PICKUP)) { + firstTimeAutoPickup = false; + firstTimeCalculated = false; + firstTimeGoingInCalculated = false; + firstTimeGoingIn = false; + hasTrack = false; + } + if (!OI.getDriverA()) { + firstTimeReef = true; + } + // Logger.recordOutput("L1 Vector", + // new Pose2d(new Translation2d(2.0, 2.0), + // new Rotation2d( + // Math.atan2(getClosestL1Points().get(0)[1] - getClosestL1Points().get( + // 1)[1], getClosestL1Points().get(0)[0] - getClosestL1Points().get(1)[0])))); + + // Logger.recordOutput("Point 1", new Pose2d(new + // Translation2d(getClosestL1Points().get(0)[0], + // getClosestL1Points().get(0)[1]), new Rotation2d(0.0))); + // Logger.recordOutput("Point 2", new Pose2d(new + // Translation2d(getClosestL1Points().get(1)[0], + // getClosestL1Points().get(1)[1]), new Rotation2d(0.0))); + + // Pose2d l1Pose = new Pose2d(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1], + // new Rotation2d(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + + switch (systemState) { + case DEFAULT: + teleopDrive(); + break; + case IDLE: + break; + case STOP: + Vector velocityVector = new Vector(); + velocityVector.setI(0); + velocityVector.setJ(0); + double desiredThetaChange = 0.0; + autoDrive(velocityVector, desiredThetaChange); + break; + case AUTO_L1: + // driveToTheta(Math.toDegrees(getThetaToCenterReef())); + // driveToTheta(Math.toDegrees(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + // driveOnLine(new Vector(getClosestL1Points().get(0)[0] - + // getClosestL1Points().get( + // 1)[0], getClosestL1Points().get(0)[1] + // - getClosestL1Points().get( + // 1)[1]), + // new Translation2d(getClosestL1Points().get(0)[0], getClosestL1Points().get( + // 0)[1]), + // getThetaToPoint(getClosestL1CornerPointXY()[0], + // getClosestL1CornerPointXY()[1])); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getL1ReefClosestSetpoint(getMT2Odometry()); + } + setpoint = getL1ReefClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case AUTO_L1_MORE: + // driveToTheta(Math.toDegrees(getThetaToCenterReef())); + // driveToTheta(Math.toDegrees(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + // driveOnLine(new Vector(getClosestL1Points().get(0)[0] - + // getClosestL1Points().get( + // 1)[0], getClosestL1Points().get(0)[1] + // - getClosestL1Points().get( + // 1)[1]), + // new Translation2d(getClosestL1Points().get(0)[0], getClosestL1Points().get( + // 0)[1]), + // getThetaToPoint(getClosestL1CornerPointXY()[0], + // getClosestL1CornerPointXY()[1])); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + // if (firstTimeReef) { + // firstTimeReef = false; + // origionalSetpointPose = getL1ReefClosestSetpoint(getMT2Odometry(), false); + // } + setpoint = getL1ReefClosestSetpointMore(getMT2Odometry()); + driveToPoint(setpoint); + break; + case REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefClosestSetpoint(getMT2Odometry(), false); + } + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case REEF_MORE: + setpoint = getReefMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case BACK: + if (autoPlacingFront) { + autoRobotCentricDrive(backupVector, 0.0); + } else { + autoRobotCentricDrive(otherBackupVector, 0.0); + } + break; + case L4_REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefL4ClosestSetpoint(getMT2Odometry(), false); + } + // System.out.println(origionalSetpointPose); + // Logger.recordOutput("Targeted L4 Point", + // getReefL4ClosestSetpoint(getMT2Odometry(), OI.getDriverA())); + // TODO: make this work + setpoint = getReefL4ClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case L3_REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefL3ClosestSetpoint(getMT2Odometry(), false); + } + setpoint = getReefL3ClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case PIECE_PICKUP: + // Pose2d target = getGamePiecePosition(); + // if (!target.equals(new Pose2d())) { + // targetPointPickup = target; + // } + // if (hasTrack) { + goToCoral(); + java.util.logging.Logger.getGlobal().finer("Driving to point"); + // } else { + // System.out.println("forward"); + // Vector v = new Vector(); + // v.setI(0.4); + // v.setJ(0); + // double d = 0.0; + // autoRobotCentricDrive(v, d); + // } + break; + case ALGAE: + setpoint = getAlgaeClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case ALGAE_MORE: + // if + // (getAngleDifferenceDegrees(Math.toDegrees(getAlgaeClosestSetpoint(getMT2Odometry())[2]), + // Math.toDegrees(getMT2OdometryAngle())) <= 90) { + // autoRobotCentricDrive(scoreL23Vector, 0); + // } else { + // autoRobotCentricDrive(scoreL23Vector, 0); + // } + setpoint = getAlgaeMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case ALGAE_MORE_MORE: + setpoint = getAlgaeMoreMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case PROCESSOR: + if (isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorBlueFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorBlueFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorBlueFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + // driveToTheta((Constants.Reef.processorBlueBackPlacingPosition.getRotation().getDegrees())); + driveToPoint(Constants.Reef.processorBlueBackPlacingPosition); + } + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorRedFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorRedFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorRedFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorRedBackPlacingPosition); + } + // driveToTheta((Constants.Reef.processorRedBackPlacingPosition.getRotation().getDegrees())); + } + } + break; + case PROCESSOR_MORE: + if (isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorMoreBlueFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreBlueFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorMoreBlueFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + // driveToTheta((Constants.Reef.processorMoreBlueBackPlacingPosition.getRotation().getDegrees())); + driveToPoint(Constants.Reef.processorMoreBlueBackPlacingPosition); + } + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorMoreRedFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreRedFrontPlacingPosition); + // driveToTheta((Constants.Reef.processorMoreRedFrontPlacingPosition.getRotation().getDegrees())); + } + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreRedBackPlacingPosition); + } + // driveToTheta((Constants.Reef.processorMoreRedBackPlacingPosition.getRotation().getDegrees())); + } + } + break; + case NET_MORE: + if (OI.getDriverA()) { + teleopDrive(); + } else if (Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + double[] setpointA = getNetMoreXTheta(); + driveToXTheta(setpointA[0], setpointA[1]); + } + + // if (OI.isBlueSide()) { // TODO: uncomment to revert to colorado algae code + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXMore, + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXMore, + // Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // } else { + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXMore, + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXMore, + // Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // } + + break; + case NET: + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + double[] setpointA = getNetXTheta(); + driveToXTheta(setpointA[0], setpointA[1]); + } + + // if (OI.isBlueSide()) { // TODO: uncomment to revert to colorado algae code + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXM, + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXM, + // Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // } else { + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXM, + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXM, + // Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // } + + break; + case SCORE_L23: + autoRobotCentricDrive(scoreL23Vector, 0); + break; + case FEEDER: + if (getFieldSide() == "red") { // red side + if (getMT2OdometryY() > 4.026) { // redside right feeder (field top right) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + driveToTheta(234); + } else { // robot back side redside left feeder (fieldside top right) + driveToTheta(54); + } + } else { // redside left feeder (fieldside bottom right) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + driveToTheta(306); + } else { // robot back side redside left (fieldside bottom right) + driveToTheta(126); + } + } + } else { // blue side + if (getMT2OdometryY() < 4.026) { // blue side right feeder (fieldside bottom left) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + driveToTheta(234); + } else { // robot back side blueside right (fieldside bottom left) + driveToTheta(54); + } + } else { // blue side left feeder (fieldside top left) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + driveToTheta(306); + } else { // robot back side blueside left (fieldside top left) + driveToTheta(126); + } + } + } + + break; + case AUTO_FEEDER: + break; + case FEEDER_ALIGN: + if (getFieldSide() == "red") { // red side + if (getMT2OdometryY() > 4.026) { // redside right feeder (field top right) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + Vector feederLine = new Vector( + -Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getRadians()); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } + } else { // redside left feeder (fieldside bottom right) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + Vector feederLine = new Vector( + -Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getRadians()); + } + } + } + } else { // blue side + if (getMT2OdometryY() < 4.026) { // blue side right feeder (fieldside bottom left) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + Vector feederLine = new Vector( + -Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getRadians()); + } + } + } else { // blue side left feeder (fieldside top left) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + Vector feederLine = new Vector( + -Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getRadians()); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } + } + } + break; + case FEEDER_AUTO: + break; + case AUTO_CLIMB: + Vector moveBack = new Vector(0.2, 0); + + if (!firstClimb) { + startX = getMT2OdometryX(); + startY = getMT2OdometryY(); + firstClimb = true; + } + + if (Math.hypot((Math.abs(getMT2OdometryX() - startX)), Math.abs(getMT2OdometryY() - startY)) < 0.15) { + autoRobotCentricDrive(moveBack, 0); + } else { + autoRobotCentricDrive(new Vector(0, 0), 0); + } + break; + + default: + + break; + } + } + + /** + * Calculates the adjusted y-coordinate based on the original x and y + * coordinates. + * + * @param originalX The original x-coordinate. + * @param originalY The original y-coordinate. + * @return The adjusted y-coordinate. + */ + public double getAdjustedY(double originalX, double originalY) { + double adjustedY = originalY * Math.sqrt((1 - (Math.pow(originalX, 2)) / 2)); + return adjustedY; + } + + /** + * Calculates the adjusted x-coordinate based on the original x and y + * coordinates. + * + * @param originalX The original x-coordinate. + * @param originalY The original y-coordinate. + * @return The adjusted x-coordinate. + */ + public double getAdjustedX(double originalX, double originalY) { + double adjustedX = originalX * Math.sqrt((1 - (Math.pow(originalY, 2)) / 2)); + return adjustedX; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java b/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java new file mode 100644 index 0000000..ae97fd7 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java @@ -0,0 +1,496 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.Constants; +import frc.robot.tools.math.Vector; + +public class Peripherals { + private PhotonCamera frontReefCam = new PhotonCamera("Front_Reef"); + private PhotonCamera frontSwerveCam = new PhotonCamera("Front_Swerve"); + private PhotonCamera backReefCam = new PhotonCamera("Back_Reef"); + private PhotonCamera backLeftReefCam = new PhotonCamera("Back_Left_Reef"); + private PhotonCamera backRightReefCam = new PhotonCamera("Back_Right_Reef"); + private PhotonCamera frontBargeCam = new PhotonCamera("Front_Barge"); + private PhotonCamera backBargeCam = new PhotonCamera("Back_Barge"); + private PhotonCamera gamePieceCamera = new PhotonCamera("Front_Game_Piece_Cam"); + + AprilTagFieldLayout aprilTagFieldLayout; + + private Pigeon2 pigeon = new Pigeon2(0, "Canivore"); + private Pigeon2 pigeonExtra = new Pigeon2(1, "Canivore"); + + private Pigeon2Configuration pigeonConfig = new Pigeon2Configuration(); + private Pigeon2Configuration pigeonExtraConfig = new Pigeon2Configuration(); + Transform3d robotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(1.75), Constants.inchesToMeters(11.625), + Constants.inchesToMeters(33.5)), + new Rotation3d(0, Math.toRadians(30.6), 0)); + PhotonPoseEstimator photonPoseEstimator; + + double pigeonSetpoint = 0.0; + + boolean frontReefCamTrack = false; + boolean backReefCamTrack = false; + boolean frontBargeCamTrack = false; + boolean backBargeCamTrack = false; + + public Peripherals() { + } + + /** + * Initializes the Peripherals subsystem. + * + * This method sets up the IMU configuration, mount pose, and zeroes the IMU. + * It also applies the default command to the Peripherals subsystem. + */ + public void init() { + try { + aprilTagFieldLayout = new AprilTagFieldLayout( + Filesystem.getDeployDirectory().getPath() + "/" + "2025-reefscape.json"); + } catch (Exception e) { + java.util.logging.Logger.getGlobal().warning("error with april tag: " + e.getMessage()); + } + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); + // Set the mount pose configuration for the IMU + pigeonConfig.MountPose.MountPosePitch = 0.3561480641365051; + pigeonConfig.MountPose.MountPoseRoll = -0.10366992652416229; + pigeonConfig.MountPose.MountPoseYaw = -0.24523599445819855; + + pigeonExtraConfig.MountPose.MountPosePitch = 2.9378318786621094; + pigeonExtraConfig.MountPose.MountPoseRoll = -1.7237101793289185; + pigeonExtraConfig.MountPose.MountPoseYaw = -1.0769075155258179; + + // Apply the IMU configuration + pigeon.getConfigurator().apply(pigeonConfig); + pigeonExtra.getConfigurator().apply(pigeonExtraConfig); + + // Zero the IMU angle + zeroPigeon(); + + setPigeonPitchOffset(getPigeonPitch()); + + } + + public double getFrontReefCamYaw() { + double yaw = 0.0; + var result = frontReefCam.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + yaw = target.getYaw(); + } + return yaw; + } + + public double getFrontReefCamPitch() { + double pitch = 0.0; + var result = frontReefCam.getLatestResult(); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + pitch = target.getPitch(); + } + + return pitch; + } + + public void setBackCamPipline(int index) { + backReefCam.setPipelineIndex(index); + } + + public void setGamePieceCamPipline(int index) { + gamePieceCamera.setPipelineIndex(index); + } + + public double getGamePieceCamYaw() { + double yaw = 0.0; + var result = gamePieceCamera.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + yaw = target.getYaw(); + } + return yaw; + } + + public List getGamePieceCamCorners() { + List corners = new ArrayList(); + var result = gamePieceCamera.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + corners = target.getDetectedCorners(); + } + return corners; + } + + public double getGamePiecePitch() { + double pitch = 0.0; + var result = gamePieceCamera.getLatestResult(); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + pitch = target.getPitch(); + } + + return pitch; + } + + // private Pose2d getRobotPoseViaTrig(PhotonTrackedTarget trackedTarget, + // double[] cameraPositionOnRobot, + // double robotAngle) { + // double pitch = trackedTarget.getPitch(); + // double yaw = trackedTarget.getYaw(); + // int id = trackedTarget.getFiducialId(); + // double cameraXOffset = cameraPositionOnRobot[0]; + // double cameraYOffset = cameraPositionOnRobot[1]; + // double cameraZOffset = cameraPositionOnRobot[2]; + // double cameraRYOffset = cameraPositionOnRobot[4]; + // double cameraRZOffset = cameraPositionOnRobot[5]; + + // double[] tagPose = Constants.Vision.TAG_POSES[id - 1]; + // double tagHeight = tagPose[2]; + // double tagX = tagPose[0]; + // double tagY = tagPose[1]; + // double tagYaw = tagPose[3]; + + // double distToTag = (tagHeight - cameraZOffset) / + // Math.tan(Math.toRadians(pitch + cameraRYOffset)); + // Logger.recordOutput("Distance to Tag", distToTag); + // Logger.recordOutput("yaw to Tag", yaw); + // double txProjOntoGroundPlane = Math.atan((Math.tan(yaw)) / Math.cos(pitch)); + // double xFromTag = distToTag * Math.cos(Math.toRadians(txProjOntoGroundPlane + + // robotAngle + cameraRZOffset)); + // double yFromTag = distToTag * Math.sin(Math.toRadians(txProjOntoGroundPlane + + // robotAngle + cameraRZOffset)); + // Logger.recordOutput("x to Tag", xFromTag); + // Logger.recordOutput("y to Tag", yFromTag); + + // double fieldPoseX = -xFromTag + tagX - cameraXOffset; + // double fieldPoseY = -yFromTag + tagY - cameraYOffset; + // Pose2d pose = new Pose2d(fieldPoseX, fieldPoseY, new + // Rotation2d(Math.toRadians(getPigeonAngle()))); + // return pose; + // } + + /** + * Calculates the robot's position based on the camera offset, angles to the + * tag, and tag position. + * + * @param cameraOffset Pose3d representing the camera's position and + * orientation relative to the robot center. + * @param horizontalAngle Horizontal angle (radians) from the camera to the tag. + * @param verticalAngle Vertical angle (radians) from the camera to the tag. + * @param tagPosition Pose3d representing the position of the tag in the + * field coordinate system. + * @param robotYaw Yaw (rotation around the Z-axis) of the robot in + * radians. + * @return Pose3d representing the robot's position in the field coordinate + * system. + */ + // public static Pose3d calculateRobotPosition( + // Pose3d cameraOffset, double horizontalAngle, double verticalAngle, Pose3d + // tagPosition, double robotYaw) { + + // // Extract the camera's offset from the robot in its local frame + // Translation3d cameraTranslation = cameraOffset.getTranslation(); + + // // Rotate the camera's offset into the field frame using the robot's yaw + // double cosYaw = Math.cos(robotYaw); + // double sinYaw = Math.sin(robotYaw); + // Translation3d cameraInFieldTranslation = new Translation3d( + // cosYaw * cameraTranslation.getX() - sinYaw * cameraTranslation.getY(), + // sinYaw * cameraTranslation.getX() + cosYaw * cameraTranslation.getY(), + // cameraTranslation.getZ()); + + // // Calculate the relative position of the tag to the camera + // double dx = tagPosition.getTranslation().getX() - + // cameraInFieldTranslation.getX(); + // double dy = tagPosition.getTranslation().getY() - + // cameraInFieldTranslation.getY(); + // double dz = tagPosition.getTranslation().getZ() - + // cameraInFieldTranslation.getZ(); + + // // Calculate the distance from the camera to the tag + // double distanceToTag = Math.sqrt(dx * dx + dy * dy + dz * dz); + + // // Translation from the camera to the tag in the camera's local space + // Translation3d tagInCameraSpace = new Translation3d( + // distanceToTag * Math.cos(horizontalAngle) * Math.cos(verticalAngle), // X + // distanceToTag * Math.sin(horizontalAngle) * Math.cos(verticalAngle), // Y + // distanceToTag * Math.sin(verticalAngle) // Z + // ); + + // // Transform the tag position from the camera space to the field space + // Translation3d tagInFieldSpace = + // cameraInFieldTranslation.plus(tagInCameraSpace); + + // // Calculate the robot's position in the field frame + // Translation3d robotTranslation = + // tagPosition.getTranslation().minus(tagInFieldSpace); + + // // Return the robot's position with its yaw in the field coordinate system + // return new Pose3d(robotTranslation, new Rotation3d(0, 0, robotYaw)); + // } + + // public Pose2d getFrontReefCamTrigPose() { + // var result = frontReefCam.getLatestResult(); + // if (result.hasTargets() && result.getBestTarget().getPoseAmbiguity() < 0.3) { + // PhotonTrackedTarget target = result.getBestTarget(); + // // Pose3d robotPose = calculateRobotPosition(cameraOffset, target.getYaw(), + // // target.getPitch(), + // // new Pose3d( + // // new Translation3d(Constants.Vision.TAG_POSES[10][0], + // // Constants.Vision.TAG_POSES[10][1], + // // Constants.Vision.TAG_POSES[10][2]), + // // new Rotation3d(0.0, Constants.Vision.TAG_POSES[10][4], + // // Constants.Vision.TAG_POSES[10][3])), + // // Math.toRadians(getPigeonAngle())); + // Pose2d robotPose = getRobotPoseViaTrig(target, + // Constants.Vision.FRONT_CAMERA_POSE, getPigeonAngle()); + // Logger.recordOutput("Trig Localiazation", robotPose); + // return robotPose; + // } else { + // Pose2d defaultPose = new Pose2d(0.0, 0.0, new Rotation2d(0.0)); + // return defaultPose; + // } + // } + + public PhotonPipelineResult getFrontReefCamResult() { + var result = frontReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + frontReefCamTrack = true; + return result.get(0); + } else { + frontReefCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackReefCamResult() { + var result = backReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + backReefCamTrack = true; + return result.get(0); + } else { + backReefCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackLeftReefCamResult() { + var result = backLeftReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackRightReefCamResult() { + var result = backRightReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontSwerveCamResult() { + var result = frontSwerveCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontBargeCamResult() { + var result = frontBargeCam.getAllUnreadResults(); + if (!result.isEmpty()) { + frontBargeCamTrack = true; + return result.get(0); + } else { + frontBargeCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackBargeCamResult() { + var result = backBargeCam.getAllUnreadResults(); + if (!result.isEmpty()) { + backBargeCamTrack = true; + return result.get(0); + } else { + backBargeCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontGamePieceCamResult() { + var result = gamePieceCamera.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + // public Pose3d getFrontReefCamPnPPose() { + // // Logger.recordOutput("April Tag", aprilTagFieldLayout.getTagPose(1).get()); + // var result = frontReefCam.getLatestResult(); + // Optional multiTagResult = + // photonPoseEstimator.update(result); + // if (multiTagResult.isPresent()) { + // Pose3d robotPose = multiTagResult.get().estimatedPose; + // Logger.recordOutput("multitag result", robotPose); + // return robotPose; + // } else { + // Pose3d robotPose = new Pose3d(); + // return robotPose; + // } + // // var result = frontReefCam.getLatestResult(); + // // Logger.recordOutput("Is presetn", result.getMultiTagResult().isPresent()); + // // if (result.getMultiTagResult().isPresent()) { + // // Transform3d fieldToCamera = + // // result.getMultiTagResult().get().estimatedPose.best; + // // Logger.recordOutput("Camera pose: ", fieldToCamera); + // // return fieldToCamera; + // // } else { + // // System.out.println("in else"); + // // return new Transform3d(); + // // } + // } + + public double getFrontReefCamLatency() { + return frontReefCam.getLatestResult().getTimestampSeconds(); + } + + /** + * Sets the IMU angle to 0 + */ + public void zeroPigeon() { + setPigeonAngle(0.0); + } + + /** + * Sets the angle of the IMU + * + * @param degrees - Angle to be set to the IMU + */ + public void setPigeonAngle(double degrees) { + pigeon.setYaw(degrees); + pigeonExtra.setYaw(degrees); + } + + /** + * Retrieves the yaw of the robot + * + * @return Yaw in degrees + */ + public double getPigeonAngle() { + return pigeon.getYaw().getValueAsDouble(); + } + + public double getPigeonExtraAngle() { + return pigeonExtra.getYaw().getValueAsDouble(); + } + + /** + * Retrieves the absolute angular velocity of the IMU's Z-axis in device + * coordinates. + * + * @return The absolute angular velocity of the IMU's Z-axis in device + * coordinates. + * The value is in degrees per second. + */ + public double getPigeonAngularVelocity() { + return Math.abs(pigeon.getAngularVelocityZDevice().getValueAsDouble()); + } + + /** + * Retrieves the absolute angular velocity of the IMU's Z-axis in world + * coordinates. + * + * @return The absolute angular velocity of the IMU's Z-axis in world + * coordinates. + * The value is in radians per second. + */ + public double getPigeonAngularVelocityW() { + return pigeon.getAngularVelocityZWorld().getValueAsDouble(); + } + + /** + * Retrieves the acceleration vector of the robot + * + * @return Current acceleration vector of the robot + */ + public Vector getPigeonLinAccel() { + Vector accelVector = new Vector(); + accelVector.setI(pigeon.getAccelerationX().getValueAsDouble() / Constants.Physical.GRAVITY_ACCEL_MS2); + accelVector.setJ(pigeon.getAccelerationY().getValueAsDouble() / Constants.Physical.GRAVITY_ACCEL_MS2); + return accelVector; + } + + public double getPigeonPitch() { + return pigeon.getPitch().getValueAsDouble(); + } + + public double getPigeonPitchAdjusted() { + return getPigeonPitch() - pigeonPitchOffset; + } + + double pigeonPitchOffset = 0.0; + + public void setPigeonPitchOffset(double newOffset) { + pigeonPitchOffset = newOffset; + } + + double cameraScreenshotTime = 0.0; + + public void periodic() { + Logger.recordOutput("Pigeon Pitch", getPigeonPitchAdjusted()); + + // Use to take snapshots of camera stream (Output means processed stream, input + // means raw stream) + // if (Timer.getFPGATimestamp() - cameraScreenshotTime > 1.0 && + // (DriverStation.isEnabled())) { + // gamePieceCamera.takeOutputSnapshot(); + // cameraScreenshotTime = Timer.getFPGATimestamp(); + // } + + // Logger.recordOutput("Pidgeon Yaw?", pigeon.getYaw().getValueAsDouble()); + // Logger.recordOutput("Pidgeon Pitch?", pigeon.getPitch().getValueAsDouble()); + // Logger.recordOutput("Pidgeon Roll?", pigeon.getRoll().getValueAsDouble()); + // TODO: uncomment if you want to see if the cameras have a track + // Logger.recordOutput("Front Cam Track", frontReefCamTrack); + // Logger.recordOutput("Back Cam Track", backReefCamTrack); + // Logger.recordOutput("Right Cam Track", frontBargeCamTrack); + // Logger.recordOutput("Left Cam Track", backBargeCamTrack); + + // getFrontReefCamPnPPose(); //TODO: uncomment when using camera + // var result = frontReefCam.getLatestResult(); + // if (result.hasTargets()) { + // PhotonTrackedTarget target = result.getBestTarget(); + // Pose2d robotPose = getRobotPoseViaTrig(target, + // Constants.Vision.FRONT_CAMERA_POSE, getPigeonAngle()); + // Logger.recordOutput("Trig Localiazation", robotPose); + // } + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java b/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..b8bbb83 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,111 @@ +// 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.subsystems; + +import java.util.function.BooleanSupplier; +import java.util.function.IntFunction; + +import javax.lang.model.util.ElementScanner14; + +import org.littletonrobotics.junction.Logger; + +import com.fasterxml.jackson.databind.ser.BeanSerializer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.OI; +import frc.robot.Robot; +import frc.robot.subsystems.Drive.DriveState; + +public class Superstructure extends SubsystemBase { + /** Creates a new Superstructure. */ + public enum SuperState { + DEFAULT, + IDLE + } + + private SuperState wantedSuperState = SuperState.IDLE; + private SuperState currentSuperState = SuperState.IDLE; + private boolean pathCompleted = false; + + public boolean algaeMode = false; + + private boolean manipulatorHasCoral = false; + private BooleanSupplier coralInManipulator = () -> manipulatorHasCoral; + + Drive drive; + Peripherals peripherals; + + public enum CurrentMode { + ALGAE, + CORAL + } + + public CurrentMode currentMode = CurrentMode.CORAL; + + public Superstructure(Drive driveSubsystem, Peripherals peripheralSubsystem) { + drive = driveSubsystem; + peripherals = peripheralSubsystem; + + } + + public BooleanSupplier hasCoralInManipulator() { + return coralInManipulator; + } + + public void setManipulatorHasCoral(boolean hasCoral) { + coralInManipulator = () -> hasCoral; + } + + public void setCurrentMode(CurrentMode mode) { + currentMode = mode; + } + + public void setWantedState(SuperState wantedState) { + System.out.println("Wanted State: " + wantedState); + this.wantedSuperState = wantedState; + + } + + public SuperState getCurrentSuperState() { + return currentSuperState; + } + + public boolean isPathCompleted() { + return pathCompleted; + } + + private double backUpTime = Timer.getFPGATimestamp(); + + private void applyStates() { + switch (currentSuperState) { + case DEFAULT: + drive.setWantedState(DriveState.DEFAULT); + break; + } + } + + private SuperState handleStateTransitions() { + switch (wantedSuperState) { + case DEFAULT: + currentSuperState = SuperState.DEFAULT; + break; + + default: + currentSuperState = SuperState.DEFAULT; + break; + } + return currentSuperState; + } + + @Override + public void periodic() { + Logger.recordOutput("SuperStructure State", currentSuperState); + currentSuperState = handleStateTransitions(); + applyStates(); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java b/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..19e6198 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,534 @@ +// 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.subsystems; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.tools.math.Vector; + +// **Zero Wheels with the bolt head showing on the left when the front side(battery) is facing down/away from you** + +public class SwerveModule extends SubsystemBase { + private final TalonFX angleMotor; + private final TalonFX driveMotor; + private final int moduleNumber; + private final CANcoder canCoder; + + PositionTorqueCurrentFOC positionTorqueFOCRequest = new PositionTorqueCurrentFOC(0); + VelocityTorqueCurrentFOC velocityTorqueFOCRequest = new VelocityTorqueCurrentFOC(0); + VelocityTorqueCurrentFOC velocityTorqueFOCRequestAngleMotor = new VelocityTorqueCurrentFOC(0); + // VelocityTorqueCurrentFOC velocityTorqueFOCRequestDriveMotorStop = new + // VelocityTorqueCurrentFOC(0); + + boolean swerveCan1; + boolean swerveCan2; + boolean swerveCan3; + boolean swerveCan4; + boolean frontRight; + boolean frontLeft; + boolean backLeft; + boolean backRight; + boolean can1; + boolean can2; + boolean can3; + boolean can4; + + /** + * Constructs a new SwerveModule instance. + * + * @param mModuleNum The module number. + * @param mAngleMotor The TalonFX motor used for controlling the angle of the + * module. + * @param mDriveMotor The TalonFX motor used for driving the module. + * @param mCanCoder The CANCoder sensor used for feedback control. + */ + public SwerveModule(int mModuleNum, TalonFX mAngleMotor, TalonFX mDriveMotor, CANcoder mCanCoder) { + // creates values for a single module + moduleNumber = mModuleNum; + angleMotor = mAngleMotor; + driveMotor = mDriveMotor; + canCoder = mCanCoder; + } + + /** + * Calculates the torque angle for the swerve module. + * + * @return The torque angle in radians. + */ + public double torqueAngle() { + // math to find turn angle + double length = Constants.Physical.ROBOT_LENGTH / 2, width = Constants.Physical.ROBOT_WIDTH / 2, angle; + length -= Constants.Physical.MODULE_OFFSET; + width -= Constants.Physical.MODULE_OFFSET; + + switch (moduleNumber) { + case 1: + angle = (Math.atan2(-width, length)) - Math.PI; + break; + case 2: + angle = Math.atan2(width, length); + break; + case 3: + angle = Math.PI + Math.atan2(width, -length); + break; + case 4: + angle = (2 * Math.PI) + (Math.atan2(-width, -length)); + break; + default: + angle = 1; + } + return angle; + } + + public void init() { + // sets all of the configurations for the motors + TalonFXConfiguration angleMotorConfig = new TalonFXConfiguration(); + TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration(); + + angleMotorConfig.Slot0.kP = 370.0; + angleMotorConfig.Slot0.kI = 0.0; + angleMotorConfig.Slot0.kD = 15; + + angleMotorConfig.Slot1.kP = 3.0; + angleMotorConfig.Slot1.kI = 0.0; + angleMotorConfig.Slot1.kD = 0.0; + + angleMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 70; + angleMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -70; + + angleMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + angleMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; + + angleMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + angleMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + angleMotorConfig.Feedback.FeedbackRemoteSensorID = canCoder.getDeviceID(); + angleMotorConfig.Feedback.SensorToMechanismRatio = 1.0; + angleMotorConfig.Feedback.RotorToSensorRatio = Constants.Ratios.STEER_GEAR_RATIO; + + driveMotorConfig.Slot0.kP = 8.0; + driveMotorConfig.Slot0.kI = 0.0; + driveMotorConfig.Slot0.kD = 0.0; + driveMotorConfig.Slot0.kV = 0.0; + + // driveMotorConfig.Slot1.kP = 4.0; + // driveMotorConfig.Slot1.kI = 0.0; + // driveMotorConfig.Slot1.kD = 0.0; + // driveMotorConfig.Slot1.kV = 0.0; + + driveMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 120; + driveMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -120; + driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveMotorConfig.CurrentLimits.StatorCurrentLimit = 120; + driveMotorConfig.CurrentLimits.SupplyCurrentLimit = 120; + + driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + driveMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; + + driveMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + double absolutePosition = canCoder.getAbsolutePosition().getValueAsDouble(); + angleMotor.setPosition(absolutePosition); + driveMotor.setPosition(0.0); + + angleMotor.getConfigurator().apply(angleMotorConfig); + driveMotor.getConfigurator().apply(driveMotorConfig); + + velocityTorqueFOCRequestAngleMotor.Slot = 1; + // velocityTorqueFOCRequestDriveMotorStop.Slot = 1; + } + + public void setDriveCurrentLimits(double supply, double stator) { + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.StatorCurrentLimitEnable = true; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + currentLimitsConfigs.StatorCurrentLimit = stator; + currentLimitsConfigs.SupplyCurrentLimit = supply; + driveMotor.getConfigurator().apply(currentLimitsConfigs); + } + + /** + * Sets the position and velocity PID for the swerve module's wheel. + * + * @param angle The desired angle for the wheel. + * @param velocity The desired velocity for the wheel. + */ + public void setWheelPID(double angle, double velocity) { + // method used to move wheel + angleMotor.setControl(positionTorqueFOCRequest.withPosition(degreesToRotations(Math.toDegrees(angle)))); + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(wheelToDriveMotorRotations(velocity))); + } + + /** + * Sets the drive motor velocity + * + * @param velocity - Wheel Velocity in RPS + */ + public void setDrivePID(double velocity) { + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(wheelToDriveMotorRotations(velocity))); + } + + /** + * Converts wheel to motor rotations for the steer motor + * + * @param rotations - Wheel angle in rotations + * @return - Motor angle in rotations + */ + public double wheelToSteerMotorRotations(double rotations) { + return (rotations * Constants.Ratios.STEER_GEAR_RATIO); + } + + /** + * Converts motor to wheel rotations for the steer motor + * + * @param rotations - Motor angle in rotations + * @return Wheel angle in rotations + */ + public double steerMotorToWheelRotations(double rotations) { + return (rotations / Constants.Ratios.STEER_GEAR_RATIO); + } + + /** + * Converts wheel to motor rotations for the drive motor + * + * @param rotations - Wheel rotations + * @return Motor rotations + */ + public double wheelToDriveMotorRotations(double rotations) { + return (rotations * Constants.Ratios.DRIVE_GEAR_RATIO); + } + + /** + * Converts wheel to motor rotations for the drive motor + * + * @param rotations - Motor rotations + * @return Wheel rotations + */ + public double driveMotorToWheelRotations(double rotations) { + return (rotations / Constants.Ratios.DRIVE_GEAR_RATIO); + } + + public double degreesToRotations(double degrees) { + return degrees / 360; + } + + public double rotationsToDegrees(double rotations) { + return rotations * 360; + } + + /** + * Ground meters to wheel rotations + * + * @param mps - Meters Per Second + * @return - Wheel Rotations per Second + */ + public double MPSToRPS(double mps) { + return (mps * Constants.Physical.WHEEL_ROTATION_PER_METER); + } + + /** + * Ground meters to wheel rotations + * + * @param rps - Meters Per Second + * @return - Wheel Rotations per Second + */ + public double RPSToMPS(double rps) { + return (rps / Constants.Physical.WHEEL_ROTATION_PER_METER); + } + + public void moveAngleMotor(double speed) { + angleMotor.set(0.2); + } + + public void moveDriveMotor(double speed) { + driveMotor.set(-0.5); + } + + /** + * Gets the distance traveled by the swerve module's wheel. + * + * @return The distance traveled by the wheel in meters. + */ + public double getModuleDistance() { + double position = driveMotor.getPosition().getValueAsDouble(); + // double wheelRotations = (position * Constants.Wheel_Rotations_In_A_Meter) / + // Constants.GEAR_RATIO; + double wheelRotations = driveMotorToWheelRotations(position); + double distance = RPSToMPS(wheelRotations); + return distance; + } + + /** + * Retrieves Wheel Angle + * + * @return Wheel angle in radians + */ + public double getWheelPosition() { + double position = angleMotor.getPosition().getValueAsDouble(); + return Constants.rotationsToRadians(position); + } + + /** + * Retrieves the current wheel velocity + * + * @return Wheel velocity in RPS + */ + public double getWheelSpeed() { + double speed = driveMotorToWheelRotations(driveMotor.getVelocity().getValueAsDouble()); + return speed; + } + + /** + * Retrieves the velocity of the angle of the wheel + * + * @return Wheel Angle Velocity in RPS + */ + public double getAngleVelocity() { + return angleMotor.getVelocity().getValueAsDouble(); + } + + /** + * Retrieves the velocity of the drive motor rotor + * + * @return Rotor velocity (RPS) + */ + public double getWheelSpeedWithoutGearRatio() { + return driveMotor.getVelocity().getValueAsDouble(); + } + + /** + * Retrieves the angle of the motor rotor + * + * @return Angle Rotor Position (radians) + */ + public double getAngleMotorPosition() { + double degrees = rotationsToDegrees(wheelToSteerMotorRotations(angleMotor.getPosition().getValueAsDouble())); + return (Math.toRadians(degrees)); + } + + /** + * Retrieves the absolute position of the CANCoder + * + * @return CANCoder absolute position (rotations) + */ + public double getCanCoderPosition() { + return canCoder.getAbsolutePosition().getValueAsDouble(); + } + + /** + * Retrieves the absolute position of the CANCoder + * + * @return CANCoder absolute position (radians) + */ + public double getCanCoderPositionRadians() { + return Constants.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble()); + } + + /** + * Retrieves the current ground speed of the swerve module + * + * @return Speed (MPS) + */ + public double getGroundSpeed() { + return RPSToMPS(getWheelSpeed()); + } + + /** + * Retrieves the setpoint of the angle motor + * + * @return Angle motor setpoint (rotations) + */ + public double getAngleMotorSetpoint() { + return angleMotor.getClosedLoopReference().getValue(); + } + + /** + * Retrieves the setpoint of the drive motor + * + * @return Drive motor setpoint (MPS) + */ + public double getDriveMotorSetpoint() { + return RPSToMPS(driveMotorToWheelRotations(driveMotor.getClosedLoopReference().getValue())); + } + + /** + * Calculates the angle of the joystick input relative to the positive y-axis. + * + * @param joystickY The y-component of the joystick input. + * @param joystickX The x-component of the joystick input. + * @return The angle of the joystick input in radians, relative to the positive + * y-axis. + */ + public double getJoystickAngle(double joystickY, double joystickX) { + double joystickAngle = Math.atan2(-joystickX, -joystickY); + return joystickAngle; + } + + /** + * Calculates the position magnitude of the joystick input. + * + * @param joystickY The y-component of the joystick input. + * @param joystickX The x-component of the joystick input. + * @return The magnitude of the joystick position. + */ + public double getJoystickPosition(double joystickY, double joystickX) { + double position = joystickY * joystickY + joystickX * joystickX; + return position; + } + + /** + * Drives the swerve module based on the given control vector, turn value, and + * current orientation. + * + * @param vector The control vector representing the desired direction and + * magnitude of movement. + * @param turnValue The turn value representing the rotation to be applied. + * @param navxAngle The current orientation angle from the IMU sensor. + */ + public void drive(Vector vector, double turnValue, double navxAngle) { + // turnValue = -turnValue; + if (Math.abs(vector.getI()) < 0.001 && Math.abs(vector.getJ()) < 0.001 && Math.abs(turnValue) < 0.01) { + // stops motors when joysticks are at 0 + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(0.0)); + // driveMotor.setControl(velocityTorqueFOCRequestDriveMotorStop.withVelocity(0.0)); + angleMotor.setControl(velocityTorqueFOCRequestAngleMotor.withVelocity(0.0)); + } else { + double angleWanted = Math.atan2(vector.getJ(), vector.getI()); + double wheelPower = Math.sqrt(Math.pow(vector.getI(), 2) + Math.pow(vector.getJ(), 2)); + + // field centric math + double angleWithNavx = angleWanted + navxAngle; + + double xValueWithNavx = wheelPower * Math.cos(angleWithNavx); + double yValueWithNavx = wheelPower * Math.sin(angleWithNavx); + + double turnX = turnValue * (Constants.angleToUnitVectorI(torqueAngle())); + double turnY = turnValue * (Constants.angleToUnitVectorJ(torqueAngle())); + + // adds turn and strafe vectors + Vector finalVector = new Vector(); + finalVector.setI(xValueWithNavx + turnX); + finalVector.setJ(yValueWithNavx + turnY); + + double finalAngle = -Math.atan2(finalVector.getJ(), finalVector.getI()); + double finalVelocity = Math.sqrt(Math.pow(finalVector.getI(), 2) + Math.pow(finalVector.getJ(), 2)); + + if (finalVelocity > Constants.Physical.TOP_SPEED) { + finalVelocity = Constants.Physical.TOP_SPEED; + } + + double velocityRPS = (MPSToRPS(finalVelocity)); + + double currentAngle = getWheelPosition(); + double currentAngleBelow360 = (getWheelPosition()) % (Math.toRadians(360)); + + // runs angle through optimizer to optimize wheel motion + double setpointAngle = findClosestAngle(currentAngleBelow360, finalAngle); + double setpointAngleFlipped = findClosestAngle(currentAngleBelow360, finalAngle + Math.PI); + + // used to make drive motor move less the more the angle motor is from its + // setpoint + double angleDifference = Math.abs(currentAngleBelow360 - finalAngle); + double adjustedVelocity = ((Math.cos(angleDifference)) * velocityRPS); + + // moves wheel + if (Math.abs(setpointAngle) <= Math.abs(setpointAngleFlipped)) { + setWheelPID(currentAngle + setpointAngle, adjustedVelocity); + } else { + setWheelPID(currentAngle + setpointAngleFlipped, adjustedVelocity); + } + } + } + + /** + * Finds the closest angle between two given angles, considering wrap-around at + * 360 degrees. + * + * @param angleA The first angle. + * @param angleB The second angle. + * @return The closest angle from angle A to angle B, taking into account + * wrap-around. + */ + public double findClosestAngle(double angleA, double angleB) { + double direction = angleB - angleA; + + if (Math.abs(direction) > Math.PI) { + direction = -(Math.signum(direction) * (2 * Math.PI)) + direction; + } + return direction; + } + + @Override + public void periodic() { + // if(driveMotor.getDeviceID() == 1 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 2 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // frontRight = true; + // // System.out.println("front right - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 3 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 4 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // frontLeft = true; + // // System.out.println("front left - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 5 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 6 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // backLeft = true; + // // System.out.println("back left - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 7 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 8 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // backRight = true; + // // System.out.println("back right - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + + // if(canCoder.getDeviceID() == 1 && canCoder.getSupplyVoltage().getValue() != 0 + // && frontRight == true){ + // swerveCan1 = true; + // // System.out.println("can 1 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 2 && + // canCoder.getSupplyVoltage().getValue() != 0 && frontLeft == true){ + // swerveCan2 = true; + // // System.out.println("can 2 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 3 && + // canCoder.getSupplyVoltage().getValue() != 0 && backLeft == true){ + // swerveCan3 = true; + // // System.out.println("can 3 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 4 && + // canCoder.getSupplyVoltage().getValue() != 0 && backRight == true){ + // swerveCan4 = true; + // // System.out.println("can 4 - " + canCoder.getSupplyVoltage().getValue()); + // } + + // SmartDashboard.putBoolean(" Swerve can(1)", swerveCan1); + // SmartDashboard.putBoolean(" Swerve can(2)", swerveCan2); + // SmartDashboard.putBoolean(" Swerve can(3)", swerveCan3); + // SmartDashboard.putBoolean(" Swerve can(4)", swerveCan4); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java new file mode 100644 index 0000000..f45af9f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java @@ -0,0 +1,41 @@ +package frc.robot.tools; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.util.ArrayList; +import java.util.List; + +import org.json.JSONArray; +import org.json.JSONObject; + +import edu.wpi.first.wpilibj.Filesystem; + +public class PathLoader { + + public static List loadPath(String relativePath) throws IOException { + File file = new File(Filesystem.getDeployDirectory() + "/" + "Paths/" + relativePath); + String content = Files.readString(file.toPath()); + System.out.println(content); + JSONObject root = new JSONObject(content); + JSONArray arr = root.getJSONArray("sampled_points"); + List points = new ArrayList<>(); + for (int i = 0; i < arr.length(); i++) { + JSONObject p = arr.getJSONObject(i); + points.add(new PosePoint( + p.getDouble("x"), + p.getDouble("y"), + p.getDouble("angle"), + p.getDouble("time"))); + } + return points; + } + + public static JSONObject getJSON(String relativePath) throws IOException { + File file = new File(Filesystem.getDeployDirectory() + "/" + "Paths/" + relativePath); + String content = Files.readString(file.toPath()); + JSONObject root = new JSONObject(content); + System.out.println(root); + return root; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java b/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java new file mode 100644 index 0000000..f46cf0f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java @@ -0,0 +1,12 @@ +package frc.robot.tools; + +public class PosePoint { + public final double x, y, theta, time; + + public PosePoint(double x, double y, double theta, double time) { + this.x = x; + this.y = y; + this.theta = theta; + this.time = time; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java b/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java new file mode 100644 index 0000000..083b805 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java @@ -0,0 +1,10 @@ +package frc.robot.tools; + +import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class TriggerButton extends Trigger { + public TriggerButton(BooleanSupplier bs) { + super(bs); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/math/PID.java b/2026-Robot/src/main/java/frc/robot/tools/math/PID.java new file mode 100644 index 0000000..0ed1a29 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/math/PID.java @@ -0,0 +1,100 @@ +package frc.robot.tools.math; + +public class PID { + private double error; + private double totalError; + private double prevError; + + private double PValue; + private double IValue; + private double DValue; + + private double maxInput; + private double minInput; + private double maxOutput = 1.0; + private double minOutput = -1.0; + + private boolean continuous = false; + private double setPoint; + private double result; + + public PID(double kp, double ki, double kd) { + PValue = kp; + IValue = ki; + DValue = kd; + totalError = 0.0; // Initialize total error + } + + public double updatePID(double value) { + error = setPoint - value; + + if (continuous) { + if (Math.abs(error) > (maxInput - minInput) / 2) { + error = error > 0 ? error - (maxInput - minInput) : error + (maxInput - minInput); + } + } + + if (Math.abs(error) < 0.1) { + totalError += error; + totalError = clamp(totalError); + } + + result = PValue * error + IValue * totalError + DValue * (error - prevError); + prevError = error; + result = clamp(result); + return result; + } + + public void setPID(double p, double i, double d) { + PValue = p; + IValue = i; + DValue = d; + } + + public void setSetPoint(double target) { + setPoint = target; + totalError = 0; + } + + public double getSetPoint() { + return setPoint; + } + + public double getResult() { + return result; + } + + public void setMaxOutput(double output) { + maxOutput = output; + } + + public void setMinOutput(double output) { + minOutput = output; + } + + public void setMinInput(double input) { + minInput = input; + } + + public void setMaxInput(double input) { + maxInput = input; + } + + public void setContinuous(boolean value) { + continuous = value; + } + + public double clamp(double input) { + if (input > maxOutput) { + return maxOutput; + } + if (input < minOutput) { + return minOutput; + } + return input; + } + + public double getError() { + return error; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java b/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java new file mode 100644 index 0000000..0da0d96 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java @@ -0,0 +1,138 @@ +// Copyrights (c) 2018-2019 FIRST, 2020 Highlanders FRC. All Rights Reserved. + +package frc.robot.tools.math; + +import edu.wpi.first.math.geometry.Translation2d; + +public class Vector { + + private double i; + private double j; + + public Vector() { + i = 0; + j = 0; + } + + public Vector(double i, double j) { + this.i = i; + this.j = j; + } + + /** + * Returns the i-component of the vector. + * + * @return The i-component of the vector. + */ + public double getI() { + return i; + } + + /** + * Returns the j-component of the vector. + * + * @return The j-component of the vector. + */ + public double getJ() { + return j; + } + + /** + * Multiplies the vector by a scalar value. + * + *

+ * This method modifies the current vector by multiplying each component (i and + * j) by the given scalar value. + * + * @param s The scalar value to multiply the vector by. + */ + public void scalarMultiple(double s) { + this.i = this.i * s; + this.j = this.j * s; + } + + /** + * Calculates the dot product of this vector with another vector. + * + *

+ * The dot product of two vectors is a scalar value that represents the product + * of their magnitudes and the cosine of the angle + * between them. It is calculated as the sum of the product of corresponding + * components of the two vectors. + * + * @param u The other vector with which to calculate the dot product. + * @return The dot product of this vector with the given vector {@code u}. + */ + public double dot(Vector u) { + return i * u.getI() + j * u.getJ(); + } + + /** + * Calculates the magnitude (or length) of the vector. + * + *

+ * The magnitude of a vector is a non-negative scalar value that represents the + * length of the vector. + * It is calculated as the square root of the sum of the squares of its + * components (i and j). + * + * @return The magnitude (or length) of the vector. + */ + public double magnitude() { + return Math.sqrt(this.dot(this)); + } + + /** + * Sets the i-component of the vector. + * + *

+ * This method modifies the current vector by setting the i-component to the + * given value. + * + * @param i The new value for the i-component of the vector. + */ + public void setI(double i) { + this.i = i; + } + + /** + * Sets the j-component of the vector. + * + *

+ * This method modifies the current vector by setting the j-component to the + * given value. + * + * @param j The new value for the j-component of the vector. + */ + public void setJ(double j) { + this.j = j; + } + + public Vector add(Vector other) { + return new Vector(this.i + other.getI(), this.j + other.getJ()); + } + + public Vector subtract(Vector other) { + return new Vector(this.i - other.getI(), this.j - other.getJ()); + } + + public Vector scaled(double scalar) { + return new Vector(this.i * scalar, this.j * scalar); + } + + public Vector projectOther(Vector other) { + double dotProduct = dot(other); + double magnitudeSquared = dot(this); + return scaled(dotProduct / magnitudeSquared); + } + + public Translation2d getClosestPointOnLine(Translation2d pointOnLine, Translation2d pointOffLine) { + Vector toPoint = new Vector(pointOffLine.getX() - pointOnLine.getX(), pointOffLine.getY() - pointOnLine.getY()); + Vector projVector = projectOther(toPoint); // Project onto the line direction + return new Translation2d(pointOnLine.getX() + projVector.i, pointOnLine.getY() + projVector.j); + } + + public Vector perpendicular() { + return new Vector(-j, i); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java b/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java new file mode 100644 index 0000000..aa65cc0 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java @@ -0,0 +1,27 @@ +// 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.tools.wrappers; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public abstract class AutoFollower extends Command { + /** Creates a new PolarTakeDrive. */ + public abstract int getPathPointIndex(); + + public abstract void initialize(); + + @Override + public abstract void execute(); + + @Override + public abstract void end(boolean interrupted); + + public abstract void from(int pointIndex, JSONObject pathJSON, int toIndex); + + @Override + abstract public boolean isFinished(); +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/AdvantageKit.json b/2026-Robot/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/2026-Robot/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json b/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 0000000..6f40c84 --- /dev/null +++ b/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,479 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/WPILibNewCommands.json b/2026-Robot/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..3718e0a --- /dev/null +++ b/2026-Robot/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/2026-Robot/vendordeps/photonlib.json b/2026-Robot/vendordeps/photonlib.json new file mode 100644 index 0000000..2d7b1d8 --- /dev/null +++ b/2026-Robot/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +} \ No newline at end of file