first commit

2026-02-19 16:33:09 -05:00
commit 6b173fab83
50 changed files with 25061 additions and 0 deletions
--- a/WPILib-License.md
+++ b/WPILib-License.md
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2026 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--- a/build.gradle
+++ b/build.gradle
@@ -0,0 +1,107 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
+}
+
+java {
+    sourceCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
+}
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamNumber()
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                                           // longer exist in deploy directory of this project
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for all targets including JNI, which will drastically impact
+// performance.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = 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()
+
+    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('src') { into 'backup/src' }
+    from('vendordeps') { into 'backup/vendordeps' }
+    from('build.gradle') { into 'backup' }
+    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'
+}
--- a/gradle/wrapper/gradle-wrapper.jar
+++ b/gradle/wrapper/gradle-wrapper.jar
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
--- a/252
+++ b/252
@@ -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" "$@"
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -0,0 +1,94 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
--- a/settings.gradle
+++ b/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2026'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name = 'frcHome'
+            url = frcHomeMaven
+        }
+    }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
--- a/src/main/deploy/example.txt
+++ b/src/main/deploy/example.txt
@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+to get a proper path relative to the deploy directory.
--- a/src/main/deploy/pathplanner/SamplePath.path
+++ b/src/main/deploy/pathplanner/SamplePath.path
@@ -0,0 +1,76 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 0.8409049675242211,
+        "y": 3.593000471715229
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8409049675242182,
+        "y": 3.593000471715229
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.0,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 3.8506044103316963
+      },
+      "nextControl": {
+        "x": 3.0,
+        "y": 3.8506044103316963
+      },
+      "holonomicAngle": 90.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [
+          "marker1"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "after",
+        "waitTime": 2.0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.0,
+        "y": 3.0
+      },
+      "prevControl": {
+        "x": 4.0,
+        "y": 3.0
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}
--- a/src/main/deploy/pathplanner/autos/1
+++ b/src/main/deploy/pathplanner/autos/1
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "test_1_Meter"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
--- a/src/main/deploy/pathplanner/autos/90
+++ b/src/main/deploy/pathplanner/autos/90
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "test_90Degree_Turn"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
--- a/src/main/deploy/pathplanner/generatedJSON/New
+++ b/src/main/deploy/pathplanner/generatedJSON/New
--- a/src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json
+++ b/src/main/deploy/pathplanner/generatedJSON/SamplePath.wpilib.json
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
--- a/src/main/deploy/pathplanner/paths/test_1_Meter.path
+++ b/src/main/deploy/pathplanner/paths/test_1_Meter.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.075,
+        "y": 4.75
+      },
+      "prevControl": {
+        "x": 2.380178890876563,
+        "y": 5.341144901610018
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}
--- a/src/main/deploy/pathplanner/paths/test_90Degree_Turn.path
+++ b/src/main/deploy/pathplanner/paths/test_90Degree_Turn.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.75,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.48768579117494,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "NEO",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
--- a/src/main/deploy/swerve/neo/controllerproperties.json
+++ b/src/main/deploy/swerve/neo/controllerproperties.json
@@ -0,0 +1,8 @@
+{
+  "angleJoystickRadiusDeadband": 0.5,
+  "heading": {
+    "p": 0.4,
+    "i": 0,
+    "d": 0.01
+  }
+}
--- a/src/main/deploy/swerve/neo/modules/backleft.json
+++ b/src/main/deploy/swerve/neo/modules/backleft.json
@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": -10.9,
+    "left": 10.9
+  },
+  "absoluteEncoderOffset": 216.38671875,
+  "drive": {
+    "type": "sparkflex_neo",
+    "id": 12,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": true,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkflex_neo",
+    "id": 22,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 32,
+    "canbus": null
+  },
+  "absoluteEncoderInverted": false
+}
--- a/src/main/deploy/swerve/neo/modules/backright.json
+++ b/src/main/deploy/swerve/neo/modules/backright.json
@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": -10.9,
+    "left": -10.9
+  },
+  "absoluteEncoderOffset": 191.6015625,
+  "drive": {
+    "type": "sparkflex_neo",
+    "id": 11,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": true,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkflex_neo",
+    "id": 21,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 31,
+    "canbus": null
+  },
+  "absoluteEncoderInverted": false
+}
--- a/src/main/deploy/swerve/neo/modules/frontleft.json
+++ b/src/main/deploy/swerve/neo/modules/frontleft.json
@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 10.9,
+    "left": 10.9
+  },
+  "absoluteEncoderOffset": 198.896484375,
+  "drive": {
+    "type": "sparkflex_neo",
+    "id": 13,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": true,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkflex_neo",
+    "id": 23,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 33,
+    "canbus": null
+  },
+  "absoluteEncoderInverted": false
+}
--- a/src/main/deploy/swerve/neo/modules/frontright.json
+++ b/src/main/deploy/swerve/neo/modules/frontright.json
@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 10.9,
+    "left": -10.9
+  },
+  "absoluteEncoderOffset": 231.064453125,
+  "drive": {
+    "type": "sparkflex_neo",
+    "id": 10,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": true,
+    "angle": true
+  },
+  "angle": {
+    "type": "sparkflex_neo",
+    "id": 20,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 30,
+    "canbus": null
+  },
+  "absoluteEncoderInverted": false
+}
--- a/src/main/deploy/swerve/neo/modules/physicalproperties.json
+++ b/src/main/deploy/swerve/neo/modules/physicalproperties.json
@@ -0,0 +1,24 @@
+{
+  "optimalVoltage": 12,
+  "robotMass": 51.75,
+  "wheelGripCoefficientOfFriction": 1.85,
+  "currentLimit": {
+    "drive": 40,
+    "angle": 40
+  },
+  "conversionFactors": {
+    "angle": {
+      "gearRatio": 21.42857143,
+      "factor": 0
+    },
+    "drive": {
+      "diameter": 4,
+      "gearRatio": 6.12,
+      "factor": 0
+    }
+  },
+  "rampRate": {
+    "drive": 0.25,
+    "angle": 0.25
+  }
+}
--- a/src/main/deploy/swerve/neo/modules/pidfproperties.json
+++ b/src/main/deploy/swerve/neo/modules/pidfproperties.json
@@ -0,0 +1,16 @@
+{
+  "drive": {
+    "p": 0.001,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  },
+  "angle": {
+    "p": 0.002,
+    "i": 0.0,
+    "d": 0.0,
+    "f": 0,
+    "iz": 0
+  }
+}
--- a/src/main/deploy/swerve/neo/swervedrive.json
+++ b/src/main/deploy/swerve/neo/swervedrive.json
@@ -0,0 +1,14 @@
+{
+  "imu": {
+    "type": "navx",
+    "id": 0,
+    "canbus": null
+  },
+  "invertedIMU": false,
+  "modules": [
+    "frontleft.json",
+    "frontright.json",
+    "backleft.json",
+    "backright.json"
+  ]
+}
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,165 @@
+// 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.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import swervelib.math.Matter;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide
+ * numerical or boolean constants. This
+ * class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do
+ * not put anything functional in this class.
+ *
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+
+    private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming");
+
+    public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
+    public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
+    public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
+    public static final double MAX_SPEED = Units.feetToMeters(14.5);
+    // Maximum speed of the robot in meters per second, used to limit acceleration.
+
+    // public static final class AutonConstants
+    // {
+    //
+    // public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0,
+    // 0);
+    // public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01);
+    // }
+
+    public static final class DrivebaseConstants {
+
+        // Hold time on motor brakes when disabled
+        public static final double WHEEL_LOCK_TIME = 10; // seconds
+    }
+
+    public static class OperatorConstants {
+
+        // Joystick Deadband
+        public static final double DEADBAND = 0.1;
+        public static final double LEFT_Y_DEADBAND = 0.1;
+        public static final double RIGHT_X_DEADBAND = 0.1;
+        public static final double TURN_CONSTANT = 6;
+    }
+
+    public static class ShooterConstants {
+        private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5)
+                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+
+        public static double SHOOTER_VELOCITY = -0.6;
+        public static double SHOOTER_POWER = -0.45;
+
+        public static void getShooterVelocity() {
+            SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5);
+        }
+
+        public static final int CENTER_SHOOTER_MOTOR_ID = 42;
+        public static final int LEFT_SHOOTER_MOTOR_ID = 41;
+        public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
+        public static final int INDEXER_MOTOR_ID = 43;
+        public static double INDEXER_MOTOR_SPEED = 0.6;
+
+        private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6)
+                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+
+        // this method called in robot periodic so values updated in elastic are
+        // constantly read and applied to RAMP_MOTOR_SPEED
+        public static void getRampMotorSpeed() {
+            INDEXER_MOTOR_SPEED = indexerAndRampSpeed.getDouble(.6);
+        }
+
+        public static final int LEFT_ACTUATOR_PWM_PORT = 1;
+        public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
+
+        public static double DESIRED_POTENTIOMETER_DISTANCE;
+
+        // TO DO: need to equation that calculates desired potentiometer distance
+    }
+
+    public static class IntakeConstants {
+        private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.4)
+                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+
+        public static double INTAKE_WHEELS_MOTOR_SPEED;
+
+        public static void getIntakeWheelsSpeed() {
+            INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.4);
+        }
+
+        public static final int INTAKE_WHEELS_MOTOR_ID = 50;
+        public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
+        public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
+        public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
+        public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
+
+        public static class IntakeRotatorPID {
+            public static final double INTAKE_ROTATOR_P = 0.05;
+            public static final double INTAKE_ROTATOR_I = 0;
+            public static final double INTAKE_ROTATOR_D = 0.001;
+        }
+
+    }
+
+    public static class RampConstants {
+        public static final int RAMP_MOTOR_ID = 45;
+        public static double RAMP_MOTOR_SPEED = .6;
+
+        // create object and a new widget under programming tab in Elastic where object
+        // retrieves value from widget
+
+    }
+
+    public static class LimeLight {
+
+        public static final String LIMELIGHT_NAME = "limelight";
+
+        // public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18,
+        // 19, 20, 21, 22 };
+
+        public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout
+                .loadField(AprilTagFields.k2026RebuiltAndymark);
+
+        public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75
+        // public static final double ROBOT_WIDTH = Units.inchesToMeters(30 +
+        // BUMPER_WIDTH); // Tis a square, don't need this
+        public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27);
+        // public static final Transform2d HALF_ROBOT = new
+        // Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d());
+
+        public static double LIMELIGHT_TY;
+    }
+
+    public static class TargetingConstants {
+        public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
+        public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
+    }
+
+    public static class ClimberConstants {
+        public static final int CLIMB_MOTOR_ID = 60;
+        public static final int RATCHET_PWM_PORT = 0;
+
+        public static final double RATCHET_UNLOCK_ANGLE = 0;
+        public static final double RATCHET_LOCK_ANGLE = 180;
+        public static final double CLIMBER_SPEED = .5;
+    }
+}
--- a/src/main/java/frc/robot/LimelightHelpers.java
+++ b/src/main/java/frc/robot/LimelightHelpers.java
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.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;
+
+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.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args)
+  {
+    RobotBase.startRobot(Robot::new);
+  }
+}
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,190 @@
+// 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 com.pathplanner.lib.auto.AutoBuilder;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.TargetingSubsystems;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as
+ * described in the TimedRobot documentation. If you change the name of this
+ * class or the package after creating this
+ * project, you must also update the build.gradle file in the project.
+ */
+public class Robot extends TimedRobot {
+    private static Robot instance;
+    private Command m_autonomousCommand;
+
+    private RobotContainer m_robotContainer;
+
+    private Timer disabledTimer;
+    private static NetworkTable table;
+    private static GenericEntry distanceFromLimelight;
+
+    public Robot() {
+        instance = this;
+    }
+
+    public static Robot getInstance() {
+        return instance;
+    }
+
+    /**
+     * This function is run when the robot is first started up and should be used
+     * for any initialization code.
+     */
+    @Override
+    public void robotInit() {
+        // Instantiate our RobotContainer. This will perform all our button bindings,
+        // and put our
+        // autonomous chooser on the dashboard.
+        m_robotContainer = new RobotContainer();
+
+        // Create a timer to disable motor brake a few seconds after disable. This will
+        // let the robot stop
+        // immediately when disabled, but then also let it be pushed more
+        disabledTimer = new Timer();
+
+        if (isSimulation()) {
+            DriverStation.silenceJoystickConnectionWarning(true);
+        }
+
+        table = NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
+        distanceFromLimelight = Shuffleboard.getTab("Programming").add("Distance from Limelight", 0).getEntry();
+    }
+
+    /**
+     * This function is called every 20 ms, no matter the mode. Use this for items
+     * like diagnostics that you want ran
+     * during disabled, autonomous, teleoperated and test.
+     *
+     * <p>
+     * This runs after the mode specific periodic functions, but before LiveWindow
+     * and
+     * SmartDashboard integrated updating.
+     */
+    @Override
+    public void robotPeriodic() {
+        // Runs the Scheduler. This is responsible for polling buttons, adding
+        // newly-scheduled
+        // commands, running already-scheduled commands, removing finished or
+        // interrupted commands,
+        // and running subsystem periodic() methods. This must be called from the
+        // robot's periodic
+        // block in order for anything in the Command-based framework to work.
+        CommandScheduler.getInstance().run();
+
+       // Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
+        Constants.IntakeConstants.getIntakeWheelsSpeed();
+        Constants.ShooterConstants.getShooterVelocity();
+        Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
+        //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
+    }
+
+    /**
+     * This function is called once each time the robot enters Disabled mode.
+     */
+    @Override
+    public void disabledInit() {
+        m_robotContainer.setMotorBrake(true);
+        disabledTimer.reset();
+        disabledTimer.start();
+    }
+
+    @Override
+    public void disabledPeriodic() {
+        if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) {
+            m_robotContainer.setMotorBrake(false);
+            disabledTimer.stop();
+            disabledTimer.reset();
+        }
+    }
+
+    /**
+     * This autonomous runs the autonomous command selected by your
+     * {@link RobotContainer} class.
+     */
+    @Override
+    public void autonomousInit() {
+        m_robotContainer.setMotorBrake(true);
+        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+        // Print the selected autonomous command upon autonomous init
+        System.out.println("Auto selected: " + m_autonomousCommand);
+
+        // schedule the autonomous command selected in the autoChooser
+        if (m_autonomousCommand != null) {
+            m_autonomousCommand.schedule();
+        }
+    }
+
+    /**
+     * This function is called periodically during autonomous.
+     */
+    @Override
+    public void autonomousPeriodic() {
+    }
+
+    @Override
+    public void teleopInit() {
+        // This makes sure that the autonomous stops running when
+        // teleop starts running. If you want the autonomous to
+        // continue until interrupted by another command, remove
+        // this line or comment it out.
+        if (m_autonomousCommand != null) {
+            m_autonomousCommand.cancel();
+        } else {
+            CommandScheduler.getInstance().cancelAll();
+        }
+    }
+
+    /**
+     * This function is called periodically during operator control.
+     */
+    @Override
+    public void teleopPeriodic() {
+    }
+
+    @Override
+    public void testInit() {
+        // Cancels all running commands at the start of test mode.
+        CommandScheduler.getInstance().cancelAll();
+    }
+
+    /**
+     * This function is called periodically during test mode.
+     */
+    @Override
+    public void testPeriodic() {
+    }
+
+    /**
+     * This function is called once when the robot is first started up.
+     */
+    @Override
+    public void simulationInit() {
+    }
+
+    /**
+     * This function is called periodically whilst in simulation.
+     */
+    @Override
+    public void simulationPeriodic() {
+    }
+
+}
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,308 @@
+// 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 com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.RobotBase;
+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.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import java.io.File;
+import java.lang.annotation.Target;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.function.DoubleSupplier;
+import frc.robot.subsystems.TargetingSubsystems;
+import swervelib.SwerveInputStream;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.ClimberSubsystem;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very
+ * little robot logic should actually be handled in the {@link Robot} periodic
+ * methods (other than the scheduler calls).
+ * Instead, the structure of the robot (including subsystems, commands, and
+ * trigger mappings) should be declared here.
+ */
+public class RobotContainer {
+
+    // Replace with CommandPS4Controller or CommandJoystick if needed
+    final CommandXboxController driverXbox = new CommandXboxController(0);
+    // The robot's subsystems and commands are defined here...
+    private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
+            "swerve/neo"));
+
+    // Establish a Sendable Chooser that will be able to be sent to the
+    // SmartDashboard, allowing selection of desired auto
+    private final SendableChooser<Command> autoChooser;
+
+    private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
+    // private final TargetingSubsystems m_TargetingSubsystems = new
+    // TargetingSubsystems();
+    private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
+    private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
+
+    /**
+     * Converts driver input into a field-relative ChassisSpeeds that is controlled
+     * by angular velocity.
+     */
+    SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
+            () -> driverXbox.getLeftY() * -1,
+            () -> driverXbox.getLeftX() * -1)
+            .withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
+            .deadband(OperatorConstants.DEADBAND)
+            .scaleTranslation(0.8)
+            .allianceRelativeControl(true);
+
+    /**
+     * Clone's the angular velocity input stream and converts it to a fieldRelative
+     * input stream.
+     */
+    SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
+            driverXbox::getRightY)
+            .headingWhile(true);
+
+    /**
+     * Clone's the angular velocity input stream and converts it to a robotRelative
+     * input stream.
+     */
+    SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
+            .allianceRelativeControl(true);
+
+    SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
+            () -> -driverXbox.getLeftY(),
+            () -> -driverXbox.getLeftX())
+            .withControllerRotationAxis(() -> driverXbox.getRawAxis(
+                    2))
+            .deadband(OperatorConstants.DEADBAND)
+            .scaleTranslation(0.8)
+            .allianceRelativeControl(true);
+    // Derive the heading axis with math!
+    SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
+            .withControllerHeadingAxis(() -> Math.sin(
+                    driverXbox.getRawAxis(
+                            2) *
+                            Math.PI)
+                    *
+                    (Math.PI *
+                            2),
+                    () -> Math.cos(
+                            driverXbox.getRawAxis(
+                                    2) *
+                                    Math.PI)
+                            *
+                            (Math.PI *
+                                    2))
+            .headingWhile(true)
+            .translationHeadingOffset(true)
+            .translationHeadingOffset(Rotation2d.fromDegrees(
+                    0));
+
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+        // Configure the trigger bindings
+        configureBindings();
+        DriverStation.silenceJoystickConnectionWarning(true);
+
+        // Create the NamedCommands that will be used in PathPlanner
+        NamedCommands.registerCommand("test", Commands.print("I EXIST"));
+
+        // Have the autoChooser pull in all PathPlanner autos as options
+        autoChooser = AutoBuilder.buildAutoChooser();
+
+        // Set the default auto (do nothing)
+        autoChooser.setDefaultOption("Do Nothing", Commands.none());
+
+        // Add a simple auto option to have the robot drive forward for 1 second then
+        // stop
+        autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
+
+        // Put the autoChooser on the SmartDashboard
+        SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    }
+
+    /**
+     * Use this method to define your trigger->command mappings. Triggers can be
+     * created via the
+     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
+     * an arbitrary predicate, or via the
+     * named factories in
+     * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
+     * for
+     * {@link CommandXboxController
+     * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
+     * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
+     * Flight joysticks}.
+     */
+    private void configureBindings() {
+        Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
+        Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
+        Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
+        Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
+                driveDirectAngle);
+        Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
+        Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
+        Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
+                driveDirectAngleKeyboard);
+
+        driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
+                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
+        driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
+                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
+        
+        // command for
+        // full shooting system including linear actuators
+        driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
+                .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
+
+        driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
+        
+        driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
+        driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
+        driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
+        driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
+        driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
+        driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
+
+
+        // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
+        driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
+        // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
+        // () -> -driverXbox.getLeftX()));
+        if (driverXbox.getRightY() < -0.4) {
+            m_ClimberSubsystem.liftRobotCommand();
+        } else if (driverXbox.getRightY() > 0.4) {
+            m_ClimberSubsystem.lowerRobotCommand();
+        } else {
+            m_ClimberSubsystem.stopClimberCommand();
+        }
+
+        // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
+
+        if (RobotBase.isSimulation()) {
+            drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
+        } else {
+            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
+        }
+
+        if (Robot.isSimulation()) {
+            Pose2d target = new Pose2d(new Translation2d(1, 4),
+                    Rotation2d.fromDegrees(90));
+            // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
+            driveDirectAngleKeyboard.driveToPose(() -> target,
+                    new ProfiledPIDController(5,
+                            0,
+                            0,
+                            new Constraints(5, 2)),
+                    new ProfiledPIDController(5,
+                            0,
+                            0,
+                            new Constraints(Units.degreesToRadians(360),
+                                    Units.degreesToRadians(180))));
+            driverXbox.start()
+                    .onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+            driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
+            driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+                    () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
+
+            // driverXbox.b().whileTrue(
+            // drivebase.driveToPose(
+            // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
+            // );
+
+        }
+        if (DriverStation.isTest()) {
+            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
+
+            driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+            driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+            driverXbox.back().whileTrue(drivebase.centerModulesCommand());
+            driverXbox.leftBumper().onTrue(Commands.none());
+            driverXbox.rightBumper().onTrue(Commands.none());
+        } else {
+            driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+            driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
+            driverXbox.start().whileTrue(Commands.none());
+            driverXbox.back().whileTrue(Commands.none());
+            driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+            driverXbox.rightBumper().onTrue(Commands.none());
+        }
+
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        // Pass in the selected auto from the SmartDashboard as our desired autnomous
+        // commmand
+        return autoChooser.getSelected();
+    }
+
+    public void setMotorBrake(boolean brake) {
+        drivebase.setMotorBrake(brake);
+    }
+
+    public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) {
+        try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) {
+            aimPIDs.setTolerance(1.0);
+
+            return Commands.run(() -> {
+
+                double xSpeed = xSup.getAsDouble();
+                double ySpeed = ySup.getAsDouble();
+
+                double rot = 0.0;
+
+                if (LimelightHelpers.getTV("limelight")) {
+                    double tx = LimelightHelpers.getTX("limelight");
+                    rot = aimPIDs.calculate(tx, 0);
+                    rot = MathUtil.clamp(rot, -1.5, 1.5);
+                }
+
+                drivebase.drive(new Translation2d(xSpeed, ySpeed), rot, false);
+            },
+                    drivebase);
+        }
+    }
+
+    public SwerveSubsystem getSwerveDriveBase() {
+        return drivebase;
+    }
+
+    public CommandXboxController getDriverXbox() {
+        return driverXbox;
+    }
+
+    public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
+            // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
+            m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
+
+}
--- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java
+++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java
@@ -0,0 +1,86 @@
+package frc.robot.commands.swervedrive.auto;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+
+
+/**
+ * Auto Balance command using a simple PID controller. Created by Team 3512
+ * <a href="https://github.com/frc3512/Robot-2023/blob/main/src/main/java/frc3512/robot/commands/AutoBalance.java">...</a>
+ */
+public class AutoBalanceCommand extends Command
+{
+
+  private final SwerveSubsystem swerveSubsystem;
+  private final PIDController   controller;
+
+  public AutoBalanceCommand(SwerveSubsystem swerveSubsystem)
+  {
+    this.swerveSubsystem = swerveSubsystem;
+    controller = new PIDController(1.0, 0.0, 0.0);
+    controller.setTolerance(1);
+    controller.setSetpoint(0.0);
+    // each subsystem used by the command must be passed into the
+    // addRequirements() method (which takes a vararg of Subsystem)
+    addRequirements(this.swerveSubsystem);
+  }
+
+  /**
+   * The initial subroutine of a command.  Called once when the command is initially scheduled.
+   */
+  @Override
+  public void initialize()
+  {
+
+  }
+
+  /**
+   * The main body of a command.  Called repeatedly while the command is scheduled. (That is, it is called repeatedly
+   * until {@link #isFinished()}) returns true.)
+   */
+  @Override
+  public void execute()
+  {
+    SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint());
+
+    double translationVal = MathUtil.clamp(controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5,
+                                           0.5);
+    swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true);
+  }
+
+  /**
+   * <p>
+   * Returns whether this command has finished. Once a command finishes -- indicated by this method returning true --
+   * the scheduler will call its {@link #end(boolean)} method.
+   * </p><p>
+   * Returning false will result in the command never ending automatically. It may still be cancelled manually or
+   * interrupted by another command. Hard coding this command to always return true will result in the command executing
+   * once and finishing immediately. It is recommended to use *
+   * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an operation.
+   * </p>
+   *
+   * @return whether this command has finished.
+   */
+  @Override
+  public boolean isFinished()
+  {
+    return controller.atSetpoint();
+  }
+
+  /**
+   * The action to take when the command ends. Called when either the command finishes normally -- that is it is called
+   * when {@link #isFinished()} returns true -- or when  it is interrupted/canceled. This is where you may want to wrap
+   * up loose ends, like shutting off a motor that was being used in the command.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  @Override
+  public void end(boolean interrupted)
+  {
+    swerveSubsystem.lock();
+  }
+}
--- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java
+++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java
@@ -0,0 +1,120 @@
+// 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.swervedrive.drivebase;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import java.util.List;
+import java.util.function.DoubleSupplier;
+import swervelib.SwerveController;
+import swervelib.math.SwerveMath;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class AbsoluteDrive extends Command
+{
+
+  private final SwerveSubsystem swerve;
+  private final DoubleSupplier  vX, vY;
+  private final DoubleSupplier headingHorizontal, headingVertical;
+  private boolean initRotation = false;
+
+  /**
+   * Used to drive a swerve robot in full field-centric mode.  vX and vY supply translation inputs, where x is
+   * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian
+   * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot
+   * will rotate to.
+   *
+   * @param swerve            The swerve drivebase subsystem.
+   * @param vX                DoubleSupplier that supplies the x-translation joystick input.  Should be in the range -1
+   *                          to 1 with deadband already accounted for.  Positive X is away from the alliance wall.
+   * @param vY                DoubleSupplier that supplies the y-translation joystick input.  Should be in the range -1
+   *                          to 1 with deadband already accounted for.  Positive Y is towards the left wall when
+   *                          looking through the driver station glass.
+   * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's heading angle. In the
+   *                          robot coordinate system, this is along the same axis as vY. Should range from -1 to 1 with
+   *                          no deadband.  Positive is towards the left wall when looking through the driver station
+   *                          glass.
+   * @param headingVertical   DoubleSupplier that supplies the vertical component of the robot's heading angle.  In the
+   *                          robot coordinate system, this is along the same axis as vX.  Should range from -1 to 1
+   *                          with no deadband. Positive is away from the alliance wall.
+   */
+  public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal,
+                       DoubleSupplier headingVertical)
+  {
+    this.swerve = swerve;
+    this.vX = vX;
+    this.vY = vY;
+    this.headingHorizontal = headingHorizontal;
+    this.headingVertical = headingVertical;
+
+    addRequirements(swerve);
+  }
+
+  @Override
+  public void initialize()
+  {
+    initRotation = true;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute()
+  {
+
+    // Get the desired chassis speeds based on a 2 joystick module.
+    ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),
+                                                         headingHorizontal.getAsDouble(),
+                                                         headingVertical.getAsDouble());
+
+    // Prevent Movement After Auto
+    if (initRotation)
+    {
+      if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0)
+      {
+        // Get the curretHeading
+        Rotation2d firstLoopHeading = swerve.getHeading();
+
+        // Set the Current Heading to the desired Heading
+        desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos());
+      }
+      //Dont Init Rotation Again
+      initRotation = false;
+    }
+
+    // Limit velocity to prevent tippy
+    Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
+    translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
+                                           Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
+                                           swerve.getSwerveDriveConfiguration());
+    SmartDashboard.putNumber("LimitedTranslation", translation.getX());
+    SmartDashboard.putString("Translation", translation.toString());
+
+    // Make the robot move
+    swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
+
+  }
+
+  // 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;
+  }
+
+
+}
--- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java
+++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java
@@ -0,0 +1,154 @@
+// 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.swervedrive.drivebase;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import swervelib.SwerveController;
+import swervelib.math.SwerveMath;
+
+/**
+ * A more advanced Swerve Control System that has 4 buttons for which direction to face
+ */
+public class AbsoluteDriveAdv extends Command
+{
+
+  private final SwerveSubsystem swerve;
+  private final DoubleSupplier  vX, vY;
+  private final DoubleSupplier  headingAdjust;
+  private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
+  private boolean resetHeading = false;
+
+  /**
+   * Used to drive a swerve robot in full field-centric mode.  vX and vY supply translation inputs, where x is
+   * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being
+   * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of
+   * ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172
+   *
+   * @param swerve        The swerve drivebase subsystem.
+   * @param vX            DoubleSupplier that supplies the x-translation joystick input.  Should be in the range -1 to 1
+   *                      with deadband already accounted for.  Positive X is away from the alliance wall.
+   * @param vY            DoubleSupplier that supplies the y-translation joystick input.  Should be in the range -1 to 1
+   *                      with deadband already accounted for.  Positive Y is towards the left wall when looking through
+   *                      the driver station glass.
+   * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be
+   *                      adjusted. Should range from -1 to 1 with deadband already accounted for.
+   * @param lookAway      Face the robot towards the opposing alliance's wall in the same direction the driver is
+   *                      facing
+   * @param lookTowards   Face the robot towards the driver
+   * @param lookLeft      Face the robot left
+   * @param lookRight     Face the robot right
+   */
+  public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust,
+                          BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft,
+                          BooleanSupplier lookRight)
+  {
+    this.swerve = swerve;
+    this.vX = vX;
+    this.vY = vY;
+    this.headingAdjust = headingAdjust;
+    this.lookAway = lookAway;
+    this.lookTowards = lookTowards;
+    this.lookLeft = lookLeft;
+    this.lookRight = lookRight;
+
+    addRequirements(swerve);
+  }
+
+  @Override
+  public void initialize()
+  {
+    resetHeading = true;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute()
+  {
+    double headingX = 0;
+    double headingY = 0;
+
+    // These are written to allow combinations for 45 angles
+    // Face Away from Drivers
+    if (lookAway.getAsBoolean())
+    {
+      headingY = -1;
+    }
+    // Face Right
+    if (lookRight.getAsBoolean())
+    {
+      headingX = 1;
+    }
+    // Face Left
+    if (lookLeft.getAsBoolean())
+    {
+      headingX = -1;
+    }
+    // Face Towards the Drivers
+    if (lookTowards.getAsBoolean())
+    {
+      headingY = 1;
+    }
+
+    // Prevent Movement After Auto
+    if (resetHeading)
+    {
+      if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0)
+      {
+        // Get the curret Heading
+        Rotation2d currentHeading = swerve.getHeading();
+
+        // Set the Current Heading to the desired Heading
+        headingX = currentHeading.getSin();
+        headingY = currentHeading.getCos();
+      }
+      //Dont reset Heading Again
+      resetHeading = false;
+    }
+
+    ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY);
+
+    // Limit velocity to prevent tippy
+    Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
+    translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
+                                           Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
+                                           swerve.getSwerveDriveConfiguration());
+    SmartDashboard.putNumber("LimitedTranslation", translation.getX());
+    SmartDashboard.putString("Translation", translation.toString());
+
+    // Make the robot move
+    if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0)
+    {
+      resetHeading = true;
+      swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true);
+    } else
+    {
+      swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
+    }
+  }
+
+  // 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;
+  }
+
+
+}
--- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java
+++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java
@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.swervedrive.drivebase;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import java.util.List;
+import java.util.function.DoubleSupplier;
+import swervelib.SwerveController;
+import swervelib.math.SwerveMath;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class AbsoluteFieldDrive extends Command
+{
+
+  private final SwerveSubsystem swerve;
+  private final DoubleSupplier  vX, vY, heading;
+
+  /**
+   * Used to drive a swerve robot in full field-centric mode.  vX and vY supply translation inputs, where x is
+   * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian
+   * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot
+   * will rotate to.
+   *
+   * @param swerve  The swerve drivebase subsystem.
+   * @param vX      DoubleSupplier that supplies the x-translation joystick input.  Should be in the range -1 to 1 with
+   *                deadband already accounted for.  Positive X is away from the alliance wall.
+   * @param vY      DoubleSupplier that supplies the y-translation joystick input.  Should be in the range -1 to 1 with
+   *                deadband already accounted for.  Positive Y is towards the left wall when looking through the driver
+   *                station glass.
+   * @param heading DoubleSupplier that supplies the robot's heading angle.
+   */
+  public AbsoluteFieldDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY,
+                            DoubleSupplier heading)
+  {
+    this.swerve = swerve;
+    this.vX = vX;
+    this.vY = vY;
+    this.heading = heading;
+
+    addRequirements(swerve);
+  }
+
+  @Override
+  public void initialize()
+  {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute()
+  {
+
+    // Get the desired chassis speeds based on a 2 joystick module.
+
+    ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),
+                                                         new Rotation2d(heading.getAsDouble() * Math.PI));
+
+    // Limit velocity to prevent tippy
+    Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
+    translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
+                                           Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
+                                           swerve.getSwerveDriveConfiguration());
+    SmartDashboard.putNumber("LimitedTranslation", translation.getX());
+    SmartDashboard.putString("Translation", translation.toString());
+
+    // Make the robot move
+    swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
+
+  }
+
+  // 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;
+  }
+
+
+}
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -0,0 +1,55 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ClimberSubsystem extends SubsystemBase{
+    private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
+    private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
+
+    public void liftRobot() {
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
+    }
+
+    public void lowerRobot() {
+        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
+    }
+
+    public void stopClimber() {
+        climberMotor.set(0);
+    }
+
+    public Command liftRobotCommand() {
+        return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
+    }
+
+    public Command lowerRobotCommand() {
+        return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
+    }
+
+    public Command stopClimberCommand() {
+        return runOnce(() -> stopClimber());
+    }
+
+    public static void toggleRatchet(boolean toggle) {
+    if (toggle == true) {
+      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
+    } else
+      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+  }
+
+  public Command toggleRatchetCommand(boolean toggle) {
+    return runOnce(() -> toggleRatchet(toggle));
+  }
+
+  @Override
+  public void periodic()
+  {
+    SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
+  }
+}
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -0,0 +1,94 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Constants;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+
+public class IntakeSubsystem extends SubsystemBase {
+
+    private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
+            MotorType.kBrushless);
+    private static SparkClosedLoopController intakeRotatorPIDController;
+    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
+
+    public IntakeSubsystem() {
+        intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+    }
+
+    public void startIntakeMotor() {
+        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
+    }
+
+    public void reverseIntakeMotor() {
+        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
+    }
+
+    public void stopIntakeMotor() {
+        intakeMotor.set(0);
+    }
+
+    public Command startIntakeMotorCommand() {
+        return runOnce(() -> startIntakeMotor());
+    }
+
+    public Command reverseIntakeMotorCommand() {
+        return runOnce(() -> reverseIntakeMotor());
+    }
+
+    public Command stopIntakeMotorCommand() {
+        return runOnce(() -> stopIntakeMotor());
+    }
+
+    public void deployIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition);
+    }
+
+    public Command deployintakeCommand() {
+        return runOnce(() -> deployIntake());
+    }
+
+    public void retractIntake() {
+        intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
+    }
+
+    public Command retractIntakeCommand() {
+        return runOnce(() -> retractIntake());
+    }
+
+    public void assistFuelIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+                ControlType.kPosition);
+    }
+
+    public Command assistFuelIntakeCommand() {
+        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
+                .andThen(new WaitCommand(2));
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
+    }
+}
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -0,0 +1,124 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import java.util.function.BooleanSupplier;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.LimelightHelpers;
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+
+public class ShooterSubsystem extends SubsystemBase {
+    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
+           // MotorType.kBrushless);
+
+    //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
+            //MotorType.kBrushless);
+
+    //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
+    //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
+
+    private static double currentPotentiometerPosition; // might need second value for the right potentiometer
+
+    public void startShooterMotors() {
+        centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+        rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
+    }
+
+    public double getShooterMotorVelocity() {
+        return leftShooterMotor.getEncoder().getVelocity();
+    }
+
+    public void startIndexerMotor() {
+        // if (LimelightHelpers.getTX("limelight") < 1.5 &&
+        // LimelightHelpers.getTX("limelight") > -1.5) {
+        indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
+        // } else
+        // indexerMotor.set(0);
+    }
+
+    /* public Command shootFuelCommand() {
+        return run(() -> startShooterMotors())
+                .until(() -> {
+                    return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
+                })
+                .andThen(() -> startIndexerMotor());
+    } */
+
+    
+      public Command shootFuelCommand() {
+      return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
+      .andThen(() -> startIndexerMotor());
+      };
+     
+
+    public void stopShooters() {
+        centerShooterMotor.set(0);
+        leftShooterMotor.set(0);
+        rightShooterMotor.set(0);
+        indexerMotor.set(0);
+    }
+
+    public Command stopShooterCommand() {
+        return runOnce(() -> stopShooters());
+    }
+
+    public void moveActuator(double desiredPotentiometerPosition) {
+        if (desiredPotentiometerPosition > currentPotentiometerPosition) {
+            //TODO: Test for positive or negative power
+            //leftActuatorMotor.set(0.1);
+            //rightActuatorMotor.set(0.1);
+        } else {
+            //leftActuatorMotor.set(-0.1);
+            //rightActuatorMotor.set(-0.1);
+        }
+    }
+
+    public void stopActuator() {
+        //leftActuatorMotor.set(0);
+        //rightActuatorMotor.set(0);
+    }
+
+    public Command moveActuatorCommand(double desiredPotentiometerPosition) {
+        return run(() -> moveActuator(desiredPotentiometerPosition))
+                .until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
+                .andThen(() -> stopActuator());
+    }
+
+    @Override
+    public void periodic() {
+        /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
+        SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
+        currentPotentiometerPosition = leftPotentiometer.get(); */
+    }
+}
--- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java
+++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java
@@ -0,0 +1,182 @@
+package frc.robot.subsystems;
+
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.path.RotationTarget;
+import com.pathplanner.lib.path.Waypoint;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+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.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.Constants;
+import frc.robot.LimelightHelpers;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.RobotContainer;
+import frc.robot.Constants;
+
+public class TargetingSubsystems extends SubsystemBase {
+    
+    PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
+    Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
+    PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
+            AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
+            BACK_LEFT_CAMERA_OFFSETS);
+    PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
+
+    public TargetingSubsystems() {
+        photonAimPIDController.enableContinuousInput(-180, 180);
+    }
+
+    Pose2d currentRobotPose;
+
+    public List<Waypoint> rightClimbWaypoints;
+
+    public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
+        GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
+        PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
+        currentRobotPose = swerveDrive.getPose();
+        rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
+                currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
+
+        PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null,
+                goalEndState);
+        goToClimbPath.preventFlipping = true;
+
+        return swerveDrive.getAutonomousCommand("goToClimbPath");
+    }
+
+    public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
+        return new RunCommand(() -> {
+         currentRobotPose = swerveDrive.getPose();
+
+            Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
+
+            double xError = errorFromDesiredPose.getX();
+            double yError = errorFromDesiredPose.getY();
+            double angleError = errorFromDesiredPose.getRotation().getRadians();
+
+            PIDController xController = new PIDController(1.5, 0, 0);
+            PIDController yController = new PIDController(1.5, 0, 0);
+            PIDController angleController = new PIDController(3.0, 0, 0);
+
+            angleController.enableContinuousInput(-Math.PI, Math.PI);
+
+            double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
+            double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
+            double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
+                    desiredPose.getRotation().getRadians());
+            
+            swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
+        }, swerveDrive);
+    }
+
+    Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { 
+        return new RunCommand(() -> {
+        double rot = 0.0;
+        var result = photonVision.getLatestResult();
+        if (result.hasTargets()) {
+            double yawError = result.getBestTarget().getYaw();
+            rot = photonAimPIDController.calculate(yawError, 0);
+        }
+
+        rot = MathUtil.clamp(rot, -3.0, 3.0);
+
+        swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
+                driverXbox.getLeftX() * -1), rot, true);
+    }, swerveDrive);
+}
+
+
+    public PhotonPoseEstimator getPhotonPoseEstimator() {
+        return photonEstimator;
+    }
+
+    // static public NetworkTable table =
+    // NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
+    // static public NetworkTableEntry ty = table.getEntry("ty");
+    // static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
+
+    // how many degrees back is your limelight rotated from perfectly vertical?
+    static double limelightMountAngleDegrees = 25.0;
+
+    // distance from the center of the Limelight lens to the floor
+    static double limelightLensHeightInches = 27.5;
+
+    // distance from the target to the floor
+    static double goalHeightInches = 44;
+
+    static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
+    static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
+
+    // calculate distance
+    static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
+            / Math.tan(angleToGoalRadians);
+
+    public static double getDistanceFromAprilTag() {
+        angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
+        angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
+        distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
+                / Math.tan(angleToGoalRadians);
+        return distanceFromLimelightToGoalInches;
+    }
+
+    public void updateRobotPose(SwerveSubsystem swerveDrive) {
+         Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
+
+        if (result.isPresent()) {
+            EstimatedRobotPose estimatedPose = result.get();
+            swerveDrive.getSwerveDrive()
+                    .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
+        }
+    }
+    @Override
+    public void periodic() {
+
+        /*
+         * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
+         * photonVision.getLatestResult().getBestTarget().getYaw());
+         * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
+         * photonVision.getLatestResult().getBestTarget().getPitch());
+         * Shuffleboard.getTab("Vision").add("Limelight TX Value",
+         * LimelightHelpers.getTX("limelight"));
+         * Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
+         * LimelightHelpers.getFiducialID("limelight"));
+         * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
+         * Shuffleboard.getTab("Vision").addCamera("Photon",
+         * "Arducam_OV9281_USB_Camera",
+         * "http://photonvision.local:5800");
+         */
+    } 
+}
+
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -0,0 +1,720 @@
+// 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.swervedrive;
+
+import static edu.wpi.first.units.Units.Meter;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.commands.PathfindingCommand;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.DriveFeedforwards;
+import com.pathplanner.lib.util.swerve.SwerveSetpoint;
+import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.Vision.Cameras;
+import java.io.File;
+import java.io.IOException;
+import java.util.Arrays;
+import java.util.Optional;
+import java.util.concurrent.atomic.AtomicReference;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import org.json.simple.parser.ParseException;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.targeting.PhotonPipelineResult;
+import swervelib.SwerveController;
+import swervelib.SwerveDrive;
+import swervelib.SwerveDriveTest;
+import swervelib.math.SwerveMath;
+import swervelib.parser.SwerveControllerConfiguration;
+import swervelib.parser.SwerveDriveConfiguration;
+import swervelib.parser.SwerveParser;
+import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
+
+public class SwerveSubsystem extends SubsystemBase {
+    /**
+     * Swerve drive object.
+     */
+    private final SwerveDrive swerveDrive;
+
+    /**
+     * Enable vision odometry updates while driving.
+     */
+    private final boolean visionDriveTest = false;
+
+    /**
+     * PhotonVision class to keep an accurate odometry.
+     */
+    private Vision vision;
+
+    /**
+     * Initialize {@link SwerveDrive} with the directory provided.
+     *
+     * @param directory Directory of swerve drive config files.
+     */
+    public SwerveSubsystem(File directory) {
+        boolean blueAlliance = false;
+        Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1),
+                Meter.of(4)),
+                Rotation2d.fromDegrees(0))
+                : new Pose2d(new Translation2d(Meter.of(16),
+                        Meter.of(4)),
+                        Rotation2d.fromDegrees(180));
+        // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary
+        // objects being created.
+        SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+        try {
+            swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose);
+            // Alternative method if you don't want to supply the conversion factor via JSON
+            // files.
+            // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
+            // angleConversionFactor, driveConversionFactor);
+        } catch (Exception e) {
+            throw new RuntimeException(e);
+        }
+        swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot
+                                                 // via
+                                                 // angle.
+        swerveDrive.setCosineCompensator(false);// !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation
+                                                // for
+                                                // simulations since it causes discrepancies not seen in real life.
+        swerveDrive.setAngularVelocityCompensation(true,
+                true,
+                0.1); // Correct for skew that gets worse as angular velocity increases. Start with a
+                      // coefficient of 0.1.
+        swerveDrive.setModuleEncoderAutoSynchronize(false,
+                1); // Enable if you want to resynchronize your absolute encoders and motor encoders
+                    // periodically when they are not moving.
+        // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
+        // over the internal encoder and push the offsets onto it. Throws warning if not
+        // possible
+        if (visionDriveTest) {
+            setupPhotonVision();
+            // Stop the odometry thread if we are using vision that way we can synchronize
+            // updates better.
+            swerveDrive.stopOdometryThread();
+        }
+        setupPathPlanner();
+    }
+
+    /**
+     * Construct the swerve drive.
+     *
+     * @param driveCfg      SwerveDriveConfiguration for the swerve.
+     * @param controllerCfg Swerve Controller.
+     */
+    public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
+        swerveDrive = new SwerveDrive(driveCfg,
+                controllerCfg,
+                Constants.MAX_SPEED,
+                new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
+                        Rotation2d.fromDegrees(0)));
+    }
+
+    /**
+     * Setup the photon vision class.
+     */
+    public void setupPhotonVision() {
+        vision = new Vision(swerveDrive::getPose, swerveDrive.field);
+    }
+
+    @Override
+    public void periodic() {
+
+        // When vision is enabled we must manually update odometry in SwerveDrive
+        if (visionDriveTest) {
+            swerveDrive.updateOdometry();
+            vision.updatePoseEstimation(swerveDrive);
+        }
+    }
+
+    @Override
+    public void simulationPeriodic() {
+    }
+
+    /**
+     * Setup AutoBuilder for PathPlanner.
+     */
+    public void setupPathPlanner() {
+        // Load the RobotConfig from the GUI settings. You should probably
+        // store this in your Constants file
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+
+            final boolean enableFeedforward = true;
+            // Configure AutoBuilder last
+            AutoBuilder.configure(
+                    this::getPose,
+                    // Robot pose supplier
+                    this::resetOdometry,
+                    // Method to reset odometry (will be called if your auto has a starting pose)
+                    this::getRobotVelocity,
+                    // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                    (speedsRobotRelative, moduleFeedForwards) -> {
+                        if (enableFeedforward) {
+                            swerveDrive.drive(
+                                    speedsRobotRelative,
+                                    swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative),
+                                    moduleFeedForwards.linearForces());
+                        } else {
+                            swerveDrive.setChassisSpeeds(speedsRobotRelative);
+                        }
+                    },
+                    // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
+                    // optionally outputs individual module feedforwards
+                    new PPHolonomicDriveController(
+                            // PPHolonomicController is the built in path following controller for holonomic
+                            // drive trains
+                            new PIDConstants(5.0, 0.0, 0.0),
+                            // Translation PID constants
+                            new PIDConstants(3.8, 0.0, 0.0)
+                    // Rotation PID constants
+                    ),
+                    config,
+                    // The robot configuration
+                    () -> {
+                        // Boolean supplier that controls when the path will be mirrored for the red
+                        // alliance
+                        // This will flip the path being followed to the red side of the field.
+                        // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                        var alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                    },
+                    this
+            // Reference to this subsystem to set requirements
+            );
+
+        } catch (Exception e) {
+            // Handle exception as needed
+            e.printStackTrace();
+        }
+
+        // Preload PathPlanner Path finding
+        // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE
+        PathfindingCommand.warmupCommand().schedule();
+    }
+
+    /**
+     * Aim the robot at the target returned by PhotonVision.
+     *
+     * @return A {@link Command} which will run the alignment.
+     */
+    public Command aimAtTarget(Cameras camera) {
+
+        return run(() -> {
+            Optional<PhotonPipelineResult> resultO = camera.getBestResult();
+            if (resultO.isPresent()) {
+                var result = resultO.get();
+                if (result.hasTargets()) {
+                    drive(getTargetSpeeds(0,
+                            0,
+                            Rotation2d.fromDegrees(result.getBestTarget()
+                                    .getYaw()))); // Not sure if this will work, more math may be required.
+                }
+            }
+        });
+    }
+
+    /**
+     * Get the path follower with events.
+     *
+     * @param pathName PathPlanner path name.
+     * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
+     */
+    public Command getAutonomousCommand(String pathName) {
+        // Create a path following command using AutoBuilder. This will also trigger
+        // event markers.
+        return new PathPlannerAuto(pathName);
+    }
+
+    /**
+     * Use PathPlanner Path finding to go to a point on the field.
+     *
+     * @param pose Target {@link Pose2d} to go to.
+     * @return PathFinding command
+     */
+    public Command driveToPose(Pose2d pose) {
+        // Create the constraints to use while pathfinding
+        PathConstraints constraints = new PathConstraints(
+                swerveDrive.getMaximumChassisVelocity(), 4.0,
+                swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
+
+        // Since AutoBuilder is configured, we can use it to build pathfinding commands
+        return AutoBuilder.pathfindToPose(
+                pose,
+                constraints,
+                edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
+        );
+    }
+
+    /**
+     * Drive with {@link SwerveSetpointGenerator} from 254, implemented by
+     * PathPlanner.
+     *
+     * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to
+     *                                  achieve.
+     * @return {@link Command} to run.
+     * @throws IOException    If the PathPlanner GUI settings is invalid
+     * @throws ParseException If PathPlanner GUI settings is nonexistent.
+     */
+    private Command driveWithSetpointGenerator(Supplier<ChassisSpeeds> robotRelativeChassisSpeed)
+            throws IOException, ParseException {
+        SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(),
+                swerveDrive.getMaximumChassisAngularVelocity());
+        AtomicReference<SwerveSetpoint> prevSetpoint = new AtomicReference<>(
+                new SwerveSetpoint(swerveDrive.getRobotVelocity(),
+                        swerveDrive.getStates(),
+                        DriveFeedforwards.zeros(swerveDrive.getModules().length)));
+        AtomicReference<Double> previousTime = new AtomicReference<>();
+
+        return startRun(() -> previousTime.set(Timer.getFPGATimestamp()),
+                () -> {
+                    double newTime = Timer.getFPGATimestamp();
+                    SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(),
+                            robotRelativeChassisSpeed.get(),
+                            newTime - previousTime.get());
+                    swerveDrive.drive(newSetpoint.robotRelativeSpeeds(),
+                            newSetpoint.moduleStates(),
+                            newSetpoint.feedforwards().linearForces());
+                    prevSetpoint.set(newSetpoint);
+                    previousTime.set(newTime);
+
+                });
+    }
+
+    /**
+     * Drive with 254's Setpoint generator; port written by PathPlanner.
+     *
+     * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds}
+     * @return Command to drive the robot using the setpoint generator.
+     */
+    public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds) {
+        try {
+            return driveWithSetpointGenerator(() -> {
+                return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
+
+            });
+        } catch (Exception e) {
+            DriverStation.reportError(e.toString(), true);
+        }
+        return Commands.none();
+
+    }
+
+    /**
+     * Command to characterize the robot drive motors using SysId
+     *
+     * @return SysId Drive Command
+     */
+    public Command sysIdDriveMotorCommand() {
+        return SwerveDriveTest.generateSysIdCommand(
+                SwerveDriveTest.setDriveSysIdRoutine(
+                        new Config(),
+                        this, swerveDrive, 12, true),
+                3.0, 5.0, 3.0);
+    }
+
+    /**
+     * Command to characterize the robot angle motors using SysId
+     *
+     * @return SysId Angle Command
+     */
+    public Command sysIdAngleMotorCommand() {
+        return SwerveDriveTest.generateSysIdCommand(
+                SwerveDriveTest.setAngleSysIdRoutine(
+                        new Config(),
+                        this, swerveDrive),
+                3.0, 5.0, 3.0);
+    }
+
+    /**
+     * Returns a Command that centers the modules of the SwerveDrive subsystem.
+     *
+     * @return a Command that centers the modules of the SwerveDrive subsystem
+     */
+    public Command centerModulesCommand() {
+        return run(() -> Arrays.asList(swerveDrive.getModules())
+                .forEach(it -> it.setAngle(0.0)));
+    }
+
+    /**
+     * Returns a Command that tells the robot to drive forward until the command
+     * ends.
+     *
+     * @return a Command that tells the robot to drive forward until the command
+     *         ends
+     */
+    public Command driveForward() {
+        return run(() -> {
+            swerveDrive.drive(new Translation2d(1, 0), 0, false, false);
+        }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false));
+    }
+
+    /**
+     * Replaces the swerve module feedforward with a new SimpleMotorFeedforward
+     * object.
+     *
+     * @param kS the static gain of the feedforward
+     * @param kV the velocity gain of the feedforward
+     * @param kA the acceleration gain of the feedforward
+     */
+    public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
+        swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
+    }
+
+    /**
+     * Command to drive the robot using translative values and heading as angular
+     * velocity.
+     *
+     * @param translationX     Translation in the X direction. Cubed for smoother
+     *                         controls.
+     * @param translationY     Translation in the Y direction. Cubed for smoother
+     *                         controls.
+     * @param angularRotationX Angular velocity of the robot to set. Cubed for
+     *                         smoother controls.
+     * @return Drive command.
+     */
+    public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY,
+            DoubleSupplier angularRotationX) {
+        return run(() -> {
+            // Make the robot move
+            swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d(
+                    translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
+                    translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8),
+                    Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(),
+                    true,
+                    false);
+        });
+    }
+
+    /**
+     * Command to drive the robot using translative values and heading as a
+     * setpoint.
+     *
+     * @param translationX Translation in the X direction. Cubed for smoother
+     *                     controls.
+     * @param translationY Translation in the Y direction. Cubed for smoother
+     *                     controls.
+     * @param headingX     Heading X to calculate angle of the joystick.
+     * @param headingY     Heading Y to calculate angle of the joystick.
+     * @return Drive command.
+     */
+    public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
+            DoubleSupplier headingY) {
+        // swerveDrive.setHeadingCorrection(true); // Normally you would want heading
+        // correction for this kind of control.
+        return run(() -> {
+
+            Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
+                    translationY.getAsDouble()), 0.8);
+
+            // Make the robot move
+            driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
+                    headingX.getAsDouble(),
+                    headingY.getAsDouble(),
+                    swerveDrive.getOdometryHeading().getRadians(),
+                    swerveDrive.getMaximumChassisVelocity()));
+        });
+    }
+
+    /**
+     * The primary method for controlling the drivebase. Takes a
+     * {@link Translation2d} and a rotation rate, and
+     * calculates and commands module states accordingly. Can use either open-loop
+     * or closed-loop velocity control for
+     * the wheel velocities. Also has field- and robot-relative modes, which affect
+     * how the translation vector is used.
+     *
+     * @param translation   {@link Translation2d} that is the commanded linear
+     *                      velocity of the robot, in meters per
+     *                      second. In robot-relative mode, positive x is torwards
+     *                      the bow (front) and positive y is
+     *                      torwards port (left). In field-relative mode, positive x
+     *                      is away from the alliance wall
+     *                      (field North) and positive y is torwards the left wall
+     *                      when looking through the driver station
+     *                      glass (field West).
+     * @param rotation      Robot angular rate, in radians per second. CCW positive.
+     *                      Unaffected by field/robot
+     *                      relativity.
+     * @param fieldRelative Drive mode. True for field-relative, false for
+     *                      robot-relative.
+     */
+    public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
+        swerveDrive.drive(translation,
+                rotation,
+                fieldRelative,
+                false); // Open loop is disabled since it shouldn't be used most of the time.
+    }
+
+    /**
+     * Drive the robot given a chassis field oriented velocity.
+     *
+     * @param velocity Velocity according to the field.
+     */
+    public void driveFieldOriented(ChassisSpeeds velocity) {
+        swerveDrive.driveFieldOriented(velocity);
+    }
+
+    /**
+     * Drive the robot given a chassis field oriented velocity.
+     *
+     * @param velocity Velocity according to the field.
+     */
+    public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity) {
+        return run(() -> {
+            swerveDrive.driveFieldOriented(velocity.get());
+        });
+    }
+
+    /**
+     * Drive according to the chassis robot oriented velocity.
+     *
+     * @param velocity Robot oriented {@link ChassisSpeeds}
+     */
+    public void drive(ChassisSpeeds velocity) {
+        swerveDrive.drive(velocity);
+    }
+
+    /**
+     * Get the swerve drive kinematics object.
+     *
+     * @return {@link SwerveDriveKinematics} of the swerve drive.
+     */
+    public SwerveDriveKinematics getKinematics() {
+        return swerveDrive.kinematics;
+    }
+
+    /**
+     * Resets odometry to the given pose. Gyro angle and module positions do not
+     * need to be reset when calling this
+     * method. However, if either gyro angle or module position is reset, this must
+     * be called in order for odometry to
+     * keep working.
+     *
+     * @param initialHolonomicPose The pose to set the odometry to
+     */
+    public void resetOdometry(Pose2d initialHolonomicPose) {
+        swerveDrive.resetOdometry(initialHolonomicPose);
+    }
+
+    /**
+     * Gets the current pose (position and rotation) of the robot, as reported by
+     * odometry.
+     *
+     * @return The robot's pose
+     */
+    public Pose2d getPose() {
+        return swerveDrive.getPose();
+    }
+
+    /**
+     * Set chassis speeds with closed-loop velocity control.
+     *
+     * @param chassisSpeeds Chassis Speeds to set.
+     */
+    public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+        swerveDrive.setChassisSpeeds(chassisSpeeds);
+    }
+
+    /**
+     * Post the trajectory to the field.
+     *
+     * @param trajectory The trajectory to post.
+     */
+    public void postTrajectory(Trajectory trajectory) {
+        swerveDrive.postTrajectory(trajectory);
+    }
+
+    /**
+     * Resets the gyro angle to zero and resets odometry to the same position, but
+     * facing toward 0.
+     */
+    public void zeroGyro() {
+        swerveDrive.zeroGyro();
+    }
+
+    /**
+     * Checks if the alliance is red, defaults to false if alliance isn't available.
+     *
+     * @return true if the red alliance, false if blue. Defaults to false if none is
+     *         available.
+     */
+    private boolean isRedAlliance() {
+        var alliance = DriverStation.getAlliance();
+        return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+    }
+
+    /**
+     * This will zero (calibrate) the robot to assume the current position is facing
+     * forward
+     * <p>
+     * If red alliance rotate the robot 180 after the drviebase zero command
+     */
+    public void zeroGyroWithAlliance() {
+        if (isRedAlliance()) {
+            zeroGyro();
+            // Set the pose 180 degrees
+            resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
+        } else {
+            zeroGyro();
+        }
+    }
+
+    /**
+     * Sets the drive motors to brake/coast mode.
+     *
+     * @param brake True to set motors to brake mode, false for coast.
+     */
+    public void setMotorBrake(boolean brake) {
+        swerveDrive.setMotorIdleMode(brake);
+    }
+
+    /**
+     * Gets the current yaw angle of the robot, as reported by the swerve pose
+     * estimator in the underlying drivebase.
+     * Note, this is not the raw gyro reading, this may be corrected from calls to
+     * resetOdometry().
+     *
+     * @return The yaw angle
+     */
+    public Rotation2d getHeading() {
+        return getPose().getRotation();
+    }
+
+    /**
+     * Get the chassis speeds based on controller input of 2 joysticks. One for
+     * speeds in which direction. The other for
+     * the angle of the robot.
+     *
+     * @param xInput   X joystick input for the robot to move in the X direction.
+     * @param yInput   Y joystick input for the robot to move in the Y direction.
+     * @param headingX X joystick which controls the angle of the robot.
+     * @param headingY Y joystick which controls the angle of the robot.
+     * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+     */
+    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) {
+        Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+        return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                scaledInputs.getY(),
+                headingX,
+                headingY,
+                getHeading().getRadians(),
+                Constants.MAX_SPEED);
+    }
+
+    /**
+     * Get the chassis speeds based on controller input of 1 joystick and one angle.
+     * Control the robot at an offset of
+     * 90deg.
+     *
+     * @param xInput X joystick input for the robot to move in the X direction.
+     * @param yInput Y joystick input for the robot to move in the Y direction.
+     * @param angle  The angle in as a {@link Rotation2d}.
+     * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+     */
+    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
+        Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+
+        return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                scaledInputs.getY(),
+                angle.getRadians(),
+                getHeading().getRadians(),
+                Constants.MAX_SPEED);
+    }
+
+    /**
+     * Gets the current field-relative velocity (x, y and omega) of the robot
+     *
+     * @return A ChassisSpeeds object of the current field-relative velocity
+     */
+    public ChassisSpeeds getFieldVelocity() {
+        return swerveDrive.getFieldVelocity();
+    }
+
+    /**
+     * Gets the current velocity (x, y and omega) of the robot
+     *
+     * @return A {@link ChassisSpeeds} object of the current velocity
+     */
+    public ChassisSpeeds getRobotVelocity() {
+        return swerveDrive.getRobotVelocity();
+    }
+
+    /**
+     * Get the {@link SwerveController} in the swerve drive.
+     *
+     * @return {@link SwerveController} from the {@link SwerveDrive}.
+     */
+    public SwerveController getSwerveController() {
+        return swerveDrive.swerveController;
+    }
+
+    /**
+     * Get the {@link SwerveDriveConfiguration} object.
+     *
+     * @return The {@link SwerveDriveConfiguration} fpr the current drive.
+     */
+    public SwerveDriveConfiguration getSwerveDriveConfiguration() {
+        return swerveDrive.swerveDriveConfiguration;
+    }
+
+    /**
+     * Lock the swerve drive to prevent it from moving.
+     */
+    public void lock() {
+        swerveDrive.lockPose();
+    }
+
+    /**
+     * Gets the current pitch angle of the robot, as reported by the imu.
+     *
+     * @return The heading as a {@link Rotation2d} angle
+     */
+    public Rotation2d getPitch() {
+        return swerveDrive.getPitch();
+    }
+
+    /**
+     * Add a fake vision reading for testing purposes.
+     */
+    public void addFakeVisionReading() {
+        swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
+    }
+
+    /**
+     * Gets the swerve drive object.
+     *
+     * @return {@link SwerveDrive}
+     */
+    public SwerveDrive getSwerveDrive() {
+        return swerveDrive;
+    }
+}
--- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
@@ -0,0 +1,629 @@
+package frc.robot.subsystems.swervedrive;
+
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.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.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import frc.robot.Robot;
+import java.awt.Desktop;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.Supplier;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import swervelib.SwerveDrive;
+import swervelib.telemetry.SwerveDriveTelemetry;
+
+
+/**
+ * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
+ * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
+ */
+public class Vision
+{
+
+  /**
+   * April Tag Field Layout of the year.
+   */
+  public static final AprilTagFieldLayout fieldLayout                     = AprilTagFieldLayout.loadField(
+      AprilTagFields.k2026RebuiltAndymark);
+  /**
+   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
+   */
+  private final       double              maximumAmbiguity                = 0.25;
+  /**
+   * Photon Vision Simulation
+   */
+  public              VisionSystemSim     visionSim;
+  /**
+   * Count of times that the odom thinks we're more than 10meters away from the april tag.
+   */
+  private             double              longDistangePoseEstimationCount = 0;
+  /**
+   * Current pose from the pose estimator using wheel odometry.
+   */
+  private             Supplier<Pose2d>    currentPose;
+  /**
+   * Field from {@link swervelib.SwerveDrive#field}
+   */
+  private             Field2d             field2d;
+
+
+  /**
+   * Constructor for the Vision class.
+   *
+   * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
+   * @param field       Current field, should be {@link SwerveDrive#field}
+   */
+  public Vision(Supplier<Pose2d> currentPose, Field2d field)
+  {
+    this.currentPose = currentPose;
+    this.field2d = field;
+
+    if (Robot.isSimulation())
+    {
+      visionSim = new VisionSystemSim("Vision");
+      visionSim.addAprilTags(fieldLayout);
+
+      for (Cameras c : Cameras.values())
+      {
+        c.addToVisionSim(visionSim);
+      }
+
+      openSimCameraViews();
+    }
+  }
+
+  /**
+   * Calculates a target pose relative to an AprilTag on the field.
+   *
+   * @param aprilTag    The ID of the AprilTag.
+   * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
+   *                    itself correctly.
+   * @return The target pose of the AprilTag.
+   */
+  public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
+  {
+    Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
+    if (aprilTagPose3d.isPresent())
+    {
+      return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
+    } else
+    {
+      throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
+    }
+
+  }
+
+  /**
+   * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
+   *
+   * @param swerveDrive {@link SwerveDrive} instance.
+   */
+  public void updatePoseEstimation(SwerveDrive swerveDrive)
+  {
+    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
+    {
+      /*
+       * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
+       * As a result, the odometry may not always be 100% accurate.
+       * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
+       * (This is why teams implement vision system to correct odometry.)
+       * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
+       */
+      visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
+    }
+    for (Cameras camera : Cameras.values())
+    {
+      Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
+      if (poseEst.isPresent())
+      {
+        var pose = poseEst.get();
+        swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
+                                         pose.timestampSeconds,
+                                         camera.curStdDevs);
+      }
+    }
+
+  }
+
+  /**
+   * Generates the estimated robot pose. Returns empty if:
+   * <ul>
+   *  <li> No Pose Estimates could be generated</li>
+   * <li> The generated pose estimate was considered not accurate</li>
+   * </ul>
+   *
+   * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
+   */
+  public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
+  {
+    Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
+    if (Robot.isSimulation())
+    {
+      Field2d debugField = visionSim.getDebugField();
+      // Uncomment to enable outputting of vision targets in sim.
+      poseEst.ifPresentOrElse(
+          est ->
+              debugField
+                  .getObject("VisionEstimation")
+                  .setPose(est.estimatedPose.toPose2d()),
+          () -> {
+            debugField.getObject("VisionEstimation").setPoses();
+          });
+    }
+    return poseEst;
+  }
+
+
+  /**
+   * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
+   * 10m for a short amount of time.
+   *
+   * @param pose Estimated robot pose.
+   * @return Could be empty if there isn't a good reading.
+   */
+  @Deprecated(since = "2024", forRemoval = true)
+  private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
+  {
+    if (pose.isPresent())
+    {
+      double bestTargetAmbiguity = 1; // 1 is max ambiguity
+      for (PhotonTrackedTarget target : pose.get().targetsUsed)
+      {
+        double ambiguity = target.getPoseAmbiguity();
+        if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
+        {
+          bestTargetAmbiguity = ambiguity;
+        }
+      }
+      //ambiguity to high dont use estimate
+      if (bestTargetAmbiguity > maximumAmbiguity)
+      {
+        return Optional.empty();
+      }
+
+      //est pose is very far from recorded robot pose
+      if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
+      {
+        longDistangePoseEstimationCount++;
+
+        //if it calculates that were 10 meter away for more than 10 times in a row its probably right
+        if (longDistangePoseEstimationCount < 10)
+        {
+          return Optional.empty();
+        }
+      } else
+      {
+        longDistangePoseEstimationCount = 0;
+      }
+      return pose;
+    }
+    return Optional.empty();
+  }
+
+
+  /**
+   * Get distance of the robot from the AprilTag pose.
+   *
+   * @param id AprilTag ID
+   * @return Distance
+   */
+  public double getDistanceFromAprilTag(int id)
+  {
+    Optional<Pose3d> tag = fieldLayout.getTagPose(id);
+    return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
+  }
+
+  /**
+   * Get tracked target from a camera of AprilTagID
+   *
+   * @param id     AprilTag ID
+   * @param camera Camera to check.
+   * @return Tracked target.
+   */
+  public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
+  {
+    PhotonTrackedTarget target = null;
+    for (PhotonPipelineResult result : camera.resultsList)
+    {
+      if (result.hasTargets())
+      {
+        for (PhotonTrackedTarget i : result.getTargets())
+        {
+          if (i.getFiducialId() == id)
+          {
+            return i;
+          }
+        }
+      }
+    }
+    return target;
+
+  }
+
+  /**
+   * Vision simulation.
+   *
+   * @return Vision Simulation
+   */
+  public VisionSystemSim getVisionSim()
+  {
+    return visionSim;
+  }
+
+  /**
+   * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
+   */
+  private void openSimCameraViews()
+  {
+    if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
+    {
+//      try
+//      {
+//        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
+//      } catch (IOException | URISyntaxException e)
+//      {
+//        e.printStackTrace();
+//      }
+    }
+  }
+
+  /**
+   * Update the {@link Field2d} to include tracked targets/
+   */
+  public void updateVisionField()
+  {
+
+    List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
+    for (Cameras c : Cameras.values())
+    {
+      if (!c.resultsList.isEmpty())
+      {
+        PhotonPipelineResult latest = c.resultsList.get(0);
+        if (latest.hasTargets())
+        {
+          targets.addAll(latest.targets);
+        }
+      }
+    }
+
+    List<Pose2d> poses = new ArrayList<>();
+    for (PhotonTrackedTarget target : targets)
+    {
+      if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
+      {
+        Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
+        poses.add(targetPose);
+      }
+    }
+
+    field2d.getObject("tracked targets").setPoses(poses);
+  }
+
+  /**
+   * Camera Enum to select each camera
+   */
+  enum Cameras
+  {
+    /**
+     * Left Camera
+     */
+    LEFT_CAM("left",
+             new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
+             new Translation3d(Units.inchesToMeters(12.056),
+                               Units.inchesToMeters(10.981),
+                               Units.inchesToMeters(8.44)),
+             VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    /**
+     * Right Camera
+     */
+    RIGHT_CAM("right",
+              new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
+              new Translation3d(Units.inchesToMeters(12.056),
+                                Units.inchesToMeters(-10.981),
+                                Units.inchesToMeters(8.44)),
+              VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    /**
+     * Center Camera
+     */
+    CENTER_CAM("center",
+               new Rotation3d(0, Units.degreesToRadians(18), 0),
+               new Translation3d(Units.inchesToMeters(-4.628),
+                                 Units.inchesToMeters(-10.687),
+                                 Units.inchesToMeters(16.129)),
+               VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
+
+    /**
+     * Latency alert to use when high latency is detected.
+     */
+    public final  Alert                        latencyAlert;
+    /**
+     * Camera instance for comms.
+     */
+    public final  PhotonCamera                 camera;
+    /**
+     * Pose estimator for camera.
+     */
+    public final  PhotonPoseEstimator          poseEstimator;
+    /**
+     * Standard Deviation for single tag readings for pose estimation.
+     */
+    private final Matrix<N3, N1>               singleTagStdDevs;
+    /**
+     * Standard deviation for multi-tag readings for pose estimation.
+     */
+    private final Matrix<N3, N1>               multiTagStdDevs;
+    /**
+     * Transform of the camera rotation and translation relative to the center of the robot
+     */
+    private final Transform3d                  robotToCamTransform;
+    /**
+     * Current standard deviations used.
+     */
+    public        Matrix<N3, N1>               curStdDevs;
+    /**
+     * Estimated robot pose.
+     */
+    public        Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
+
+    /**
+     * Simulated camera instance which only exists during simulations.
+     */
+    public        PhotonCameraSim              cameraSim;
+    /**
+     * Results list to be updated periodically and cached to avoid unnecessary queries.
+     */
+    public        List<PhotonPipelineResult>   resultsList       = new ArrayList<>();
+    /**
+     * Last read from the camera timestamp to prevent lag due to slow data fetches.
+     */
+    private       double                       lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
+
+    /**
+     * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
+     * estimation noise on an actual robot.
+     *
+     * @param name                  Name of the PhotonVision camera found in the PV UI.
+     * @param robotToCamRotation    {@link Rotation3d} of the camera.
+     * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
+     * @param singleTagStdDevs      Single AprilTag standard deviations of estimated poses from the camera.
+     * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
+     */
+    Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
+            Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
+    {
+      latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
+
+      camera = new PhotonCamera(name);
+
+      // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html
+      robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
+
+      poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
+                                              PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                                              robotToCamTransform);
+      poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+      this.singleTagStdDevs = singleTagStdDevs;
+      this.multiTagStdDevs = multiTagStdDevsMatrix;
+
+      if (Robot.isSimulation())
+      {
+        SimCameraProperties cameraProp = new SimCameraProperties();
+        // A 640 x 480 camera with a 100 degree diagonal FOV.
+        cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
+        // Approximate detection noise with average and standard deviation error in pixels.
+        cameraProp.setCalibError(0.25, 0.08);
+        // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+        cameraProp.setFPS(30);
+        // The average and standard deviation in milliseconds of image data latency.
+        cameraProp.setAvgLatencyMs(35);
+        cameraProp.setLatencyStdDevMs(5);
+
+        cameraSim = new PhotonCameraSim(camera, cameraProp);
+        cameraSim.enableDrawWireframe(true);
+      }
+    }
+
+    /**
+     * Add camera to {@link VisionSystemSim} for simulated photon vision.
+     *
+     * @param systemSim {@link VisionSystemSim} to use.
+     */
+    public void addToVisionSim(VisionSystemSim systemSim)
+    {
+      if (Robot.isSimulation())
+      {
+        systemSim.addCamera(cameraSim, robotToCamTransform);
+      }
+    }
+
+    /**
+     * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
+     * recent result!
+     *
+     * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
+     */
+    public Optional<PhotonPipelineResult> getBestResult()
+    {
+      if (resultsList.isEmpty())
+      {
+        return Optional.empty();
+      }
+
+      PhotonPipelineResult bestResult       = resultsList.get(0);
+      double               amiguity         = bestResult.getBestTarget().getPoseAmbiguity();
+      double               currentAmbiguity = 0;
+      for (PhotonPipelineResult result : resultsList)
+      {
+        currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
+        if (currentAmbiguity < amiguity && currentAmbiguity > 0)
+        {
+          bestResult = result;
+          amiguity = currentAmbiguity;
+        }
+      }
+      return Optional.of(bestResult);
+    }
+
+    /**
+     * Get the latest result from the current cache.
+     *
+     * @return Empty optional if nothing is found. Latest result if something is there.
+     */
+    public Optional<PhotonPipelineResult> getLatestResult()
+    {
+      return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
+    }
+
+    /**
+     * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
+     * cache of results.
+     *
+     * @return Estimated pose.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
+    {
+      updateUnreadResults();
+      return estimatedRobotPose;
+    }
+
+    /**
+     * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
+     */
+    private void updateUnreadResults()
+    {
+      double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
+      
+      for (PhotonPipelineResult result : resultsList)
+      {
+        mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
+      }
+
+        resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
+        resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
+          return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
+        });
+        if (!resultsList.isEmpty())
+        {
+          updateEstimatedGlobalPose();
+        }
+
+    }
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
+     * per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link Cameras#updateEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
+     * estimation.
+     */
+    private void updateEstimatedGlobalPose()
+    {
+      Optional<EstimatedRobotPose> visionEst = Optional.empty();
+      for (var change : resultsList)
+      {
+        visionEst = poseEstimator.update(change);
+        updateEstimationStdDevs(visionEst, change.getTargets());
+      }
+      estimatedRobotPose = visionEst;
+    }
+
+    /**
+     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
+     * on number of tags, estimation strategy, and distance from the tags.
+     *
+     * @param estimatedPose The estimated pose to guess standard deviations for.
+     * @param targets       All targets in this camera frame
+     */
+    private void updateEstimationStdDevs(
+        Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
+    {
+      if (estimatedPose.isEmpty())
+      {
+        // No pose input. Default to single-tag std devs
+        curStdDevs = singleTagStdDevs;
+
+      } else
+      {
+        // Pose present. Start running Heuristic
+        var    estStdDevs = singleTagStdDevs;
+        int    numTags    = 0;
+        double avgDist    = 0;
+
+        // Precalculation - see how many tags we found, and calculate an average-distance metric
+        for (var tgt : targets)
+        {
+          var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+          if (tagPose.isEmpty())
+          {
+            continue;
+          }
+          numTags++;
+          avgDist +=
+              tagPose
+                  .get()
+                  .toPose2d()
+                  .getTranslation()
+                  .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+        }
+
+        if (numTags == 0)
+        {
+          // No tags visible. Default to single-tag std devs
+          curStdDevs = singleTagStdDevs;
+        } else
+        {
+          // One or more tags visible, run the full heuristic.
+          avgDist /= numTags;
+          // Decrease std devs if multiple targets are visible
+          if (numTags > 1)
+          {
+            estStdDevs = multiTagStdDevs;
+          }
+          // Increase std devs based on (average) distance
+          if (numTags == 1 && avgDist > 4)
+          {
+            estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+          } else
+          {
+            estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+          }
+          curStdDevs = estStdDevs;
+        }
+      }
+    }
+  }
+}
--- a/vendordeps/PathplannerLib-2026.1.2.json
+++ b/vendordeps/PathplannerLib-2026.1.2.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2026.1.2.json",
+    "name": "PathplannerLib",
+    "version": "2026.1.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2026",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2026.1.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2026.1.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
--- a/vendordeps/Phoenix5-replay-5.36.0.json
+++ b/vendordeps/Phoenix5-replay-5.36.0.json
@@ -0,0 +1,221 @@
+{
+    "fileName": "Phoenix5-replay-5.36.0.json",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.36.0",
+    "frcYear": "2026",
+    "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2026-latest.json",
+    "requires": [
+        {
+            "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
+            "offlineFileName": "Phoenix6-replay-frc2026-latest.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json"
+        }
+    ],
+    "conflictsWith": [
+        {
+            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+            "errorMessage": "Users must use the regular Phoenix 5 vendordep when using the regular Phoenix 6 vendordep.",
+            "offlineFileName": "Phoenix6-frc2026-latest.json"
+        },
+        {
+            "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+            "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
+            "offlineFileName": "Phoenix5-frc2026-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.36.0"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.36.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.36.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.replay",
+            "artifactId": "cci-replay",
+            "version": "5.36.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.36.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.36.0",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.36.0",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.36.0",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.replay",
+            "artifactId": "wpiapi-cpp-replay",
+            "version": "5.36.0",
+            "libName": "CTRE_Phoenix_WPIReplay",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.replay",
+            "artifactId": "api-cpp-replay",
+            "version": "5.36.0",
+            "libName": "CTRE_PhoenixReplay",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.replay",
+            "artifactId": "cci-replay",
+            "version": "5.36.0",
+            "libName": "CTRE_PhoenixCCIReplay",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.36.0",
+            "libName": "CTRE_Phoenix_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.36.0",
+            "libName": "CTRE_PhoenixSim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.36.0",
+            "libName": "CTRE_PhoenixCCISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
--- a/vendordeps/Phoenix6-replay-26.1.0.json
+++ b/vendordeps/Phoenix6-replay-26.1.0.json
@@ -0,0 +1,497 @@
+{
+    "fileName": "Phoenix6-replay-26.1.0.json",
+    "name": "CTRE-Phoenix (v6) Replay",
+    "version": "26.1.0",
+    "frcYear": "2026",
+    "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json",
+    "conflictsWith": [
+        {
+            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+            "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+            "offlineFileName": "Phoenix6-frc2026-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "26.1.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "api-cpp",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.replay",
+            "artifactId": "api-cpp-replay",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.replay",
+            "artifactId": "tools-replay",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANrange",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "26.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "26.1.0",
+            "libName": "CTRE_Phoenix6_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "26.1.0",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.replay",
+            "artifactId": "wpiapi-cpp-replay",
+            "version": "26.1.0",
+            "libName": "CTRE_Phoenix6_WPIReplay",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.replay",
+            "artifactId": "tools-replay",
+            "version": "26.1.0",
+            "libName": "CTRE_PhoenixTools_Replay",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "26.1.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": "26.1.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": "26.1.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": "26.1.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": "26.1.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": "simProTalonFX",
+            "version": "26.1.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": "26.1.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": "26.1.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": "26.1.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": "26.1.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": "26.1.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": "26.1.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,133 @@
+{
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2026.0.2",
+    "frcYear": "2026",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2026.0.2"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2026.0.2",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "RevLibBackendDriver",
+            "version": "2026.0.2",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "RevLibWpiBackendDriver",
+            "version": "2026.0.2",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2026.0.2",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2026.0.2",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "RevLibBackendDriver",
+            "version": "2026.0.2",
+            "libName": "BackendDriver",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "RevLibWpiBackendDriver",
+            "version": "2026.0.2",
+            "libName": "REVLibWpi",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
--- a/vendordeps/ReduxLib-2026.1.1.json
+++ b/vendordeps/ReduxLib-2026.1.1.json
@@ -0,0 +1,69 @@
+{
+  "fileName": "ReduxLib-2026.1.1.json",
+  "name": "ReduxLib",
+  "version": "2026.1.1",
+  "frcYear": "2026",
+  "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
+  "mavenUrls": [
+    "https://maven.reduxrobotics.com/"
+  ],
+  "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json",
+  "javaDependencies": [
+    {
+      "groupId": "com.reduxrobotics.frc",
+      "artifactId": "ReduxLib-java",
+      "version": "2026.1.1"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "groupId": "com.reduxrobotics.frc",
+      "artifactId": "ReduxLib-fifo",
+      "version": "2026.1.1",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "linuxathena",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal",
+        "windowsx86-64"
+      ]
+    }
+  ],
+  "cppDependencies": [
+    {
+      "groupId": "com.reduxrobotics.frc",
+      "artifactId": "ReduxLib-cpp",
+      "version": "2026.1.1",
+      "libName": "ReduxLib",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal",
+        "windowsx86-64"
+      ]
+    },
+    {
+      "groupId": "com.reduxrobotics.frc",
+      "artifactId": "ReduxLib-fifo",
+      "version": "2026.1.1",
+      "libName": "reduxfifo",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal",
+        "windowsx86-64"
+      ]
+    }
+  ]
+}
--- a/vendordeps/Studica-2026.0.0.json
+++ b/vendordeps/Studica-2026.0.0.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "Studica.json",
+    "name": "Studica",
+    "version": "2026.0.0",
+    "frcYear": "2026",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2026/"
+    ],
+    "jsonUrl": "https://dev.studica.com/maven/release/2026/json/Studica-2026.0.0.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.studica.frc",
+            "artifactId": "Studica-java",
+            "version": "2026.0.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.studica.frc",
+            "artifactId": "Studica-driver",
+            "version": "2026.0.0",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.studica.frc",
+            "artifactId": "Studica-cpp",
+            "version": "2026.0.0",
+            "libName": "Studica",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.studica.frc",
+            "artifactId": "Studica-driver",
+            "version": "2026.0.0",
+            "libName": "StudicaDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
--- a/vendordeps/ThriftyLib-2026.json
+++ b/vendordeps/ThriftyLib-2026.json
@@ -0,0 +1,20 @@
+{
+    "fileName": "ThriftyLib-2026.json",
+    "name": "ThriftyLib",
+    "version": "2026.0.1",
+    "frcYear": "2026",
+    "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
+    "mavenUrls": [
+        "https://docs.home.thethriftybot.com"
+    ],
+    "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib-2026.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.thethriftybot.frc",
+            "artifactId": "ThriftyLib-java",
+            "version": "2026.0.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}
--- a/vendordeps/WPILibNewCommands.json
+++ b/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,39 @@
+{
+    "fileName": "WPILibNewCommands.json",
+    "name": "WPILib-New-Commands",
+    "version": "1.0.0",
+    "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+    "frcYear": "2026",
+    "mavenUrls": [],
+    "jsonUrl": "",
+    "javaDependencies": [
+        {
+            "groupId": "edu.wpi.first.wpilibNewCommands",
+            "artifactId": "wpilibNewCommands-java",
+            "version": "wpilib"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "edu.wpi.first.wpilibNewCommands",
+            "artifactId": "wpilibNewCommands-cpp",
+            "version": "wpilib",
+            "libName": "wpilibNewCommands",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxsystemcore",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "windowsx86-64",
+                "windowsx86",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2026.2.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2026",
+    "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": "v2026.2.2",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2026.2.2",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2026.2.2",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2026.2.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2026.2.2"
+        }
+    ]
+}
--- a/vendordeps/yagsl-2026.1.14.json
+++ b/vendordeps/yagsl-2026.1.14.json
@@ -0,0 +1,58 @@
+{
+    "fileName": "yagsl-2026.1.14.json",
+    "name": "YAGSL",
+    "version": "2026.1.14",
+    "frcYear": "2026",
+    "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400",
+    "mavenUrls": [
+        "https://yet-another-software-suite.github.io/YAGSL/releases/",
+        "https://repo1.maven.org/maven2"
+    ],
+    "jsonUrl": "https://yet-another-software-suite.github.io/YAGSL/yagsl.json",
+    "javaDependencies": [
+        {
+            "groupId": "swervelib",
+            "artifactId": "YAGSL-java",
+            "version": "2026.1.14"
+        },
+        {
+            "groupId": "org.dyn4j",
+            "artifactId": "dyn4j",
+            "version": "5.0.2"
+        }
+    ],
+    "requires": [
+        {
+            "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
+            "errorMessage": "ReduxLib is required!",
+            "offlineFileName": "ReduxLib.json",
+            "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json"
+        },
+        {
+            "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+            "errorMessage": "REVLib is required!",
+            "offlineFileName": "REVLib.json",
+            "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json"
+        },
+        {
+            "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+            "errorMessage": "Phoenix6 Replay is required!",
+            "offlineFileName": "Phoenix6-replay-26.1.0.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json"
+        },
+        {
+            "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
+            "errorMessage": "Phoenix5 Replay Compatibility is required!",
+            "offlineFileName": "Phoenix5-replay-5.36.0.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2026-latest.json"
+        },
+        {
+            "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
+            "errorMessage": "ThriftyLib is required!",
+            "offlineFileName": "ThriftyLib.json",
+            "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib-2026.json"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}