diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..5528d4f
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,178 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+*-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000..c9c9713
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..4ed293b
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,29 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true,
+    "**/*~": true
+  },
+  "java.test.config": [
+    {
+      "name": "WPIlibUnitTests",
+      "workingDirectory": "${workspaceFolder}/build/jni/release",
+      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
+      "env": {
+        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
+        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+      }
+    },
+  ],
+  "java.test.defaultConfig": "WPIlibUnitTests"
+}
diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
new file mode 100644
index 0000000..69b76fe
--- /dev/null
+++ b/.wpilib/wpilib_preferences.json
@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2024",
+    "teamNumber": 5427
+}
\ No newline at end of file
diff --git a/WPILib-License.md b/WPILib-License.md
new file mode 100644
index 0000000..43b62ec
--- /dev/null
+++ b/WPILib-License.md
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2023 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/build.gradle b/build.gradle
new file mode 100644
index 0000000..5b29be2
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,101 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2024.1.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'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 5.
+dependencies {
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+}
+
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    from sourceSets.main.allSource
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..d64cd49
Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 0000000..5e82d67
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/gradlew b/gradlew
new file mode 100644
index 0000000..1aa94a4
--- /dev/null
+++ b/gradlew
@@ -0,0 +1,249 @@
+#!/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.
+#
+
+##############################################################################
+#
+#   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/subprojects/plugins/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 "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/gradlew.bat b/gradlew.bat
new file mode 100644
index 0000000..93e3f59
--- /dev/null
+++ b/gradlew.bat
@@ -0,0 +1,92 @@
+@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
+
+@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.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/settings.gradle b/settings.gradle
new file mode 100644
index 0000000..d94f73c
--- /dev/null
+++ b/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2024'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt
new file mode 100644
index 0000000..bb82515
--- /dev/null
+++ b/src/main/deploy/example.txt
@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+to get a proper path relative to the deploy directory.
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto
new file mode 100644
index 0000000..762d0cb
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Test.auto
@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "abcd"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..bab0da9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/abcd.path b/src/main/deploy/pathplanner/paths/abcd.path
new file mode 100644
index 0000000..8b26eaf
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/abcd.path
@@ -0,0 +1,104 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7401886415755699,
+        "y": 4.345350325316036
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.618501382509183,
+        "y": 4.859997166499325
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.474096832809052,
+        "y": 4.345350325316036
+      },
+      "prevControl": {
+        "x": 2.238794669897034,
+        "y": 5.067969559016041
+      },
+      "nextControl": {
+        "x": 6.109562171782717,
+        "y": 3.8166438579754125
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.432018229421076,
+        "y": 0.8671430804400075
+      },
+      "prevControl": {
+        "x": 5.158176374045059,
+        "y": 2.1774926242160166
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 90.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0.5,
+      "maxWaypointRelativePos": 1.5,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 10.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 1000.0
+      }
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 1.0,
+      "command": {
+        "type": "sequential",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "PrintOut"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 10.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 1000.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 5.931526924919756,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 117.95096902789015,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
new file mode 100644
index 0000000..fe215d7
--- /dev/null
+++ b/src/main/java/frc/robot/Main.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+  private Main() {}
+
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..b68462c
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  @Override
+  public void robotInit() {
+    m_robotContainer = new RobotContainer();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+  }
+
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void disabledExit() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void autonomousExit() {}
+
+  @Override
+  public void teleopInit() {
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void teleopExit() {}
+
+  @Override
+  public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  @Override
+  public void testPeriodic() {}
+
+  @Override
+  public void testExit() {}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644
index 0000000..618f2da
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+
+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.button.CommandXboxController;
+import frc.robot.io.PilotingControls;
+import frc.robot.subsystems.Swerve.SwerveDrivetrain;
+import frc.robot.util.AutonUtil;
+import frc.robot.util.SteelTalonsLogger;
+import frc.robot.util.Localization.SteelTalonsLocalization;
+
+public class RobotContainer {
+
+  private SwerveDrivetrain drivetrain;
+  private SendableChooser<Command> autoChooser;
+
+  public RobotContainer() {
+    drivetrain = new SwerveDrivetrain();
+    drivetrain.setDefaultCommand(drivetrain.getDriveCommand());
+
+    new SteelTalonsLocalization(); //has to be after drivetrain
+    new SteelTalonsLogger();
+    new AutonUtil(); //has to be last
+
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    new PilotingControls(new CommandXboxController(0));
+  }
+
+  public Command getAutonomousCommand() {
+    return autoChooser.getSelected();
+  }
+}
diff --git a/src/main/java/frc/robot/commandsAuton/AutonSheet.java b/src/main/java/frc/robot/commandsAuton/AutonSheet.java
new file mode 100644
index 0000000..4f01993
--- /dev/null
+++ b/src/main/java/frc/robot/commandsAuton/AutonSheet.java
@@ -0,0 +1,5 @@
+package frc.robot.commandsAuton;
+
+public class AutonSheet {
+    
+}
diff --git a/src/main/java/frc/robot/io/EmergencyControls.java b/src/main/java/frc/robot/io/EmergencyControls.java
new file mode 100644
index 0000000..be0cc5d
--- /dev/null
+++ b/src/main/java/frc/robot/io/EmergencyControls.java
@@ -0,0 +1,5 @@
+package frc.robot.io;
+
+public class EmergencyControls {
+    
+}
diff --git a/src/main/java/frc/robot/io/OperatingControls.java b/src/main/java/frc/robot/io/OperatingControls.java
new file mode 100644
index 0000000..e932b7d
--- /dev/null
+++ b/src/main/java/frc/robot/io/OperatingControls.java
@@ -0,0 +1,5 @@
+package frc.robot.io;
+
+public class OperatingControls {
+    
+}
diff --git a/src/main/java/frc/robot/io/PilotingControls.java b/src/main/java/frc/robot/io/PilotingControls.java
new file mode 100644
index 0000000..e6270f8
--- /dev/null
+++ b/src/main/java/frc/robot/io/PilotingControls.java
@@ -0,0 +1,21 @@
+package frc.robot.io;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.util.MiscUtil;
+import frc.robot.util.Localization.SteelTalonsLocalization;
+
+public class PilotingControls {
+    public PilotingControls(CommandXboxController controller) {
+
+        controller.y().onTrue(new InstantCommand(() -> {
+            SteelTalonsLocalization.getInstance().resetPose(
+                MiscUtil.isBlue() ? MiscUtil.resetPose()[0] : MiscUtil.resetPose()[1]
+            );
+        }, SteelTalonsLocalization.getInstance()));
+
+        
+
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java
new file mode 100644
index 0000000..f458a11
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java
@@ -0,0 +1,30 @@
+package frc.robot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.SteelTalonsSparkMaxServo;
+
+public class Arm extends SubsystemBase {
+
+    private SteelTalonsSparkMaxServo armMotor;
+
+    private double setpoint;
+    private double armFF;
+    
+    public Arm() {
+        ArmConstants.configureArmMotor();
+        armMotor = new SteelTalonsSparkMaxServo(ArmConstants.ARM_MASTER);
+
+        armFF = 0.0;
+        setpoint = 0.0;
+    }
+
+    public void setSetpoint(double setpoint) {
+        this.setpoint = setpoint;
+    }
+
+    @Override
+    public void periodic() {
+        armMotor.setSetpoint(setpoint, armFF);
+        armMotor.log();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java
new file mode 100644
index 0000000..283ab3d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java
@@ -0,0 +1,30 @@
+package frc.robot.subsystems.Arm;
+
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import frc.robot.util.STSmaxConfig;
+
+public class ArmConstants {
+    
+    public static STSmaxConfig ARM_MASTER = new STSmaxConfig();
+
+    private static final int ARM_MOTOR_ID = 0;
+
+    public static void configureArmMotor() {
+        ARM_MASTER.name = "Arm Motor";
+        ARM_MASTER.gearing = 0.0/0.0;
+        ARM_MASTER.inverted = false;
+        ARM_MASTER.id = ARM_MOTOR_ID;
+
+        ARM_MASTER.kP = 0.0;
+        ARM_MASTER.kD = 0.0;
+
+        ARM_MASTER.idleMode = IdleMode.kBrake;
+        ARM_MASTER.isRotational = true;
+        
+        ARM_MASTER.maxVel = (5676 * ARM_MASTER.gearing * 2 * Math.PI) / (60.0);
+        ARM_MASTER.maxAccel = ARM_MASTER.maxVel * 4;
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
new file mode 100644
index 0000000..1ec7887
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
@@ -0,0 +1,31 @@
+package frc.robot.subsystems.Elevator;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.SteelTalonsSparkMaxServo;
+
+public class Elevator extends SubsystemBase {
+
+    private SteelTalonsSparkMaxServo elevatorMotor;
+
+    private double setpoint;
+    private double elevatorFF;
+
+    public Elevator() {
+        ElevatorConstants.configureElevatorMotor();
+        elevatorMotor = new SteelTalonsSparkMaxServo(ElevatorConstants.ELEVATOR_MASTER);
+
+        elevatorFF = 0.0;
+        setpoint = 0.0;
+    }
+
+    public void setSetpoint(double setpoint) {
+        this.setpoint = setpoint;
+    }
+
+    @Override
+    public void periodic() {
+        elevatorMotor.setSetpoint(setpoint, elevatorFF);
+        elevatorMotor.log();
+    }
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java
new file mode 100644
index 0000000..f537c09
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java
@@ -0,0 +1,29 @@
+package frc.robot.subsystems.Elevator;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import frc.robot.util.STSmaxConfig;
+
+public class ElevatorConstants {
+    
+    public static STSmaxConfig ELEVATOR_MASTER = new STSmaxConfig();
+
+    private static final int ELEVATOR_MOTOR_ID = 0;
+
+    public static void configureElevatorMotor() {
+        ELEVATOR_MASTER.name = "Elevator Motor";
+        ELEVATOR_MASTER.gearing = 0.0/0.0f;
+        ELEVATOR_MASTER.inverted = false;
+        ELEVATOR_MASTER.id = ELEVATOR_MOTOR_ID;
+
+        ELEVATOR_MASTER.kP = 0.0;
+        ELEVATOR_MASTER.kD = 0.0;
+
+        ELEVATOR_MASTER.isRotational = false;
+        ELEVATOR_MASTER.idleMode = IdleMode.kBrake;
+
+        ELEVATOR_MASTER.maxVel = (5676 * ELEVATOR_MASTER.gearing * 2 * Math.PI) / (60.0);
+        ELEVATOR_MASTER.maxAccel = ELEVATOR_MASTER.maxVel * 4;
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/Misc/ObjectDetector.java b/src/main/java/frc/robot/subsystems/Misc/ObjectDetector.java
new file mode 100644
index 0000000..e522616
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Misc/ObjectDetector.java
@@ -0,0 +1,45 @@
+package frc.robot.subsystems.Misc;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ObjectDetector extends SubsystemBase {
+    private NetworkTable table_m;
+    private boolean tv;
+
+    private double ledMode;
+
+    public ObjectDetector(NetworkTable table) {
+        this.table_m = table;
+    }
+
+    @Override
+    public void periodic() {
+        tv = table_m.getEntry("tv").getDouble(0) == 1;
+    }
+
+    public boolean targetVisible() {
+        return tv;
+    }
+
+    public boolean lightOn() {
+        return ledMode == 0;
+    }
+
+    public double[] targetInfo() { // X - Y - A - L
+        return new double[] {
+            table_m.getEntry("tx").getDouble(0),
+            table_m.getEntry("ty").getDouble(0),
+            table_m.getEntry("ta").getDouble(0),
+            table_m.getEntry("tl").getDouble(0),
+        };
+    }
+
+    public void setLight(boolean on) {
+        if (on) {
+            table_m.getEntry("ledMode").setNumber(0);
+        } else {
+            table_m.getEntry("ledMode").setNumber(1);
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java
new file mode 100644
index 0000000..5432bd8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java
@@ -0,0 +1,17 @@
+package frc.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Shooter extends SubsystemBase {
+  /** Creates a new Shooter. */
+  public Shooter() {
+    
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+}
+
diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java
new file mode 100644
index 0000000..0c515ed
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.Shooter;
+
+public class ShooterConstants {
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/SubsystemManager.java b/src/main/java/frc/robot/subsystems/SubsystemManager.java
new file mode 100644
index 0000000..114c004
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/SubsystemManager.java
@@ -0,0 +1,7 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SubsystemManager extends SubsystemBase {
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve/DriveCommand.java b/src/main/java/frc/robot/subsystems/Swerve/DriveCommand.java
new file mode 100644
index 0000000..e018eed
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Swerve/DriveCommand.java
@@ -0,0 +1,28 @@
+package frc.robot.subsystems.Swerve;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+public class DriveCommand extends Command {
+
+    private CommandXboxController controller;
+    private SwerveDrivetrain swerve;
+
+    public DriveCommand(SwerveDrivetrain swerve, CommandXboxController controller) {
+        addRequirements(swerve);
+        this.controller = controller;
+        this.swerve = swerve;
+    }
+
+    @Override
+    public void execute() { //ADD ALL TELEOP MANIPULATION USING speeds.plus()
+        ChassisSpeeds speeds = swerve.getDriveSpeeds(controller);
+        swerve.setSetpoint(speeds);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve/DrivetrainConstants.java b/src/main/java/frc/robot/subsystems/Swerve/DrivetrainConstants.java
new file mode 100644
index 0000000..322092f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Swerve/DrivetrainConstants.java
@@ -0,0 +1,144 @@
+package frc.robot.subsystems.Swerve;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.STSmaxConfig;
+
+public class DrivetrainConstants {
+    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(3.88); //3.88
+    public static final double TRACKWIDTH = Units.inchesToMeters(19.5);
+    public static final double WHEELBASE = Units.inchesToMeters(19.5);
+
+    public static final SwerveDriveKinematics SWERVE_DRIVE_KINEMATICS = new SwerveDriveKinematics(
+            new Translation2d(WHEELBASE / 2, TRACKWIDTH / 2),
+            new Translation2d(WHEELBASE / 2, -TRACKWIDTH / 2),
+            new Translation2d(-WHEELBASE / 2, TRACKWIDTH / 2),
+            new Translation2d(-WHEELBASE / 2, -TRACKWIDTH / 2));
+
+    public static final int PIGEON_CAN_ID = 16;
+
+    public static final double MAX_TRANSLATION_SPEED_M_S_TELEOP = Units.feetToMeters(16.2);
+    public static final double MAX_ROTATION_SPEED_RAD_S_TELEOP = 8 * Math.PI;
+
+    public static final double MAX_PHYSICAL_SPEED_M_S = (5800 * (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0) * WHEEL_DIAMETER_METERS * Math.PI) / (60.0);
+    public static final double MAX_ACCEL = MAX_PHYSICAL_SPEED_M_S * 5;
+    public static final double THRESHOLD_STOPPING_M_S = 0.3;
+
+    public static final int FRONT_LEFT_CANCODER_ID = 12;
+    public static final int FRONT_RIGHT_CANCODER_ID = 13;
+    public static final int BACK_LEFT_CANCODER_ID = 14;
+    public static final int BACK_RIGHT_CANCODER_ID = 15;
+
+    public static final double FRONT_LEFT_OFFSET = 0.0;
+    public static final double FRONT_RIGHT_OFFSET = 0.0;
+    public static final double BACK_LEFT_OFFSET = 0.0;
+    public static final double BACK_RIGHT_OFFSET = 0.0;
+
+    public static final STSmaxConfig FRONT_LEFT_DRIVE = new STSmaxConfig();
+    public static final STSmaxConfig FRONT_RIGHT_DRIVE = new STSmaxConfig();
+    public static final STSmaxConfig BACK_LEFT_DRIVE = new STSmaxConfig();
+    public static final STSmaxConfig BACK_RIGHT_DRIVE = new STSmaxConfig();
+
+    public static final STSmaxConfig FRONT_LEFT_STEER = new STSmaxConfig();
+    public static final STSmaxConfig FRONT_RIGHT_STEER = new STSmaxConfig();
+    public static final STSmaxConfig BACK_LEFT_STEER = new STSmaxConfig();
+    public static final STSmaxConfig BACK_RIGHT_STEER = new STSmaxConfig();
+
+    public static void configureMotors() {
+        FRONT_LEFT_DRIVE.name = "FRONT_LEFT_DRIVE";
+        FRONT_RIGHT_DRIVE.name = "FRONT_RIGHT_DRIVE";
+        BACK_LEFT_DRIVE.name = "BACK_LEFT_DRIVE";
+        BACK_RIGHT_DRIVE.name = "BACK_RIGHT_DRIVE";
+
+        FRONT_LEFT_STEER.name = "FRONT_LEFT_STEER";
+        FRONT_RIGHT_STEER.name = "FRONT_RIGHT_STEER";
+        BACK_LEFT_STEER.name = "BACK_LEFT_STEER";
+        BACK_RIGHT_STEER.name = "BACK_RIGHT_STEER";
+
+        FRONT_LEFT_DRIVE.id = 5;
+        FRONT_RIGHT_DRIVE.id = 3;
+        BACK_LEFT_DRIVE.id = 9;
+        BACK_RIGHT_DRIVE.id = 7;
+
+        FRONT_LEFT_STEER.id = 6;
+        FRONT_RIGHT_STEER.id = 4;
+        BACK_LEFT_STEER.id = 10;
+        BACK_RIGHT_STEER.id = 8;
+    }
+
+    // public static void configureDriveTalon(TalonFX motor) {
+    //     Slot0Configs velConstants = new Slot0Configs();
+    //     velConstants.kP = 0.0; //FIXME
+    //     velConstants.kS = 0.5; //FIXME
+    //     velConstants.kV = 12 / (MAX_PHYSICAL_SPEED_M_S);
+    //     motor.getConfigurator().apply(velConstants);
+
+    //     FeedbackConfigs feedbackConfigs = new FeedbackConfigs();
+    //     feedbackConfigs.SensorToMechanismRatio = (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0) * WHEEL_DIAMETER_METERS * Math.PI;
+    //     motor.getConfigurator().apply(feedbackConfigs);
+
+    //     ClosedLoopRampsConfigs rampConfigs = new ClosedLoopRampsConfigs();
+    //     rampConfigs.VoltageClosedLoopRampPeriod = 0.2; //secs it takes to ramp to max
+    //     motor.getConfigurator().apply(rampConfigs);
+
+    //     CurrentLimitsConfigs currConfigs = new CurrentLimitsConfigs();
+    //     currConfigs.StatorCurrentLimitEnable = true;
+    //     currConfigs.SupplyCurrentLimitEnable = true;
+    //     currConfigs.StatorCurrentLimit = 60;
+    //     currConfigs.SupplyCurrentLimit = 60;
+    //     currConfigs.SupplyCurrentThreshold = 80;
+    //     currConfigs.SupplyTimeThreshold = 1.5;
+    //     motor.getConfigurator().apply(currConfigs);
+
+    //     motor.setNeutralMode(NeutralModeValue.Brake);
+    // }
+
+    public static STSmaxConfig configureDriveNeo(STSmaxConfig config) {
+        
+        config.kP = 0.05;
+        config.kD = 0.0;
+
+        config.currentLimit = 80;
+
+        config.isRotational = false;
+
+        config.gearing = (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0);
+        config.finalDiameterMeters = WHEEL_DIAMETER_METERS;
+        // config.maxVel = (5676 * config.gearing * 2 * Math.PI) / (60.0);
+        // config.maxAccel = config.maxVel * 4;
+
+        config.idleMode = IdleMode.kBrake;
+        return config;
+    }
+
+    public static STSmaxConfig configureSteerNeo(STSmaxConfig config) {
+        config.currentLimit = 20;
+        config.gearing = (1.0 / (150.0/7.0));
+        config.isRotational = true;
+        config.kP = 2;
+        config.kD = 0.0;
+        // config.maxVel = (5676 * config.gearing * 2 * Math.PI) / (60.0);
+        config.maxVel = 4800;
+        config.maxAccel = config.maxVel * 4;
+        config.inverted = true;
+        config.idleMode = IdleMode.kCoast;
+        return config;
+    }
+
+    public static void configureCanCoder(CANcoder encoder, double offset) {
+        CANcoderConfiguration config = new CANcoderConfiguration();
+        config.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
+        config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
+        config.MagnetSensor.MagnetOffset = offset;
+
+        encoder.getConfigurator().apply(config);
+    }
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/Swerve/SwerveDrivetrain.java
new file mode 100644
index 0000000..fcb313f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Swerve/SwerveDrivetrain.java
@@ -0,0 +1,158 @@
+package frc.robot.subsystems.Swerve;
+
+import java.util.List;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.util.MiscUtil;
+import frc.robot.util.SteelTalonsLogger;
+
+public class SwerveDrivetrain extends SubsystemBase {
+    
+    public static SwerveDrivetrain instance;
+    private Pigeon2 gyro;
+    private List<SwerveModule> modules;
+    private ChassisSpeeds setPoint = new ChassisSpeeds(); // X: m/s - Y: m/s - Theta: rad/s
+
+    public SwerveDrivetrain() {
+        instance = this;
+        gyro = new Pigeon2(DrivetrainConstants.PIGEON_CAN_ID);
+        gyro.reset();
+        modules = List.of(
+            new SwerveModule(
+                DrivetrainConstants.FRONT_LEFT_DRIVE, 
+                DrivetrainConstants.FRONT_LEFT_STEER, 
+                DrivetrainConstants.FRONT_LEFT_CANCODER_ID,
+                DrivetrainConstants.FRONT_LEFT_OFFSET
+            ), //FRONT LEFT
+            new SwerveModule(
+                DrivetrainConstants.FRONT_RIGHT_DRIVE, 
+                DrivetrainConstants.FRONT_RIGHT_STEER, 
+                DrivetrainConstants.FRONT_RIGHT_CANCODER_ID,
+                DrivetrainConstants.FRONT_RIGHT_OFFSET
+            ), //FRONT RIGHT
+            new SwerveModule(
+                DrivetrainConstants.BACK_LEFT_DRIVE, 
+                DrivetrainConstants.BACK_LEFT_STEER, 
+                DrivetrainConstants.BACK_LEFT_CANCODER_ID,
+                DrivetrainConstants.BACK_LEFT_OFFSET
+            ), //BACK LEFT
+            new SwerveModule(
+                DrivetrainConstants.BACK_RIGHT_DRIVE, 
+                DrivetrainConstants.BACK_RIGHT_STEER, 
+                DrivetrainConstants.BACK_RIGHT_CANCODER_ID,
+                DrivetrainConstants.BACK_RIGHT_OFFSET
+            ) //BACK RIGHT
+        );
+
+    }
+
+    public static SwerveDrivetrain getInstance() {
+        return instance;
+    }
+
+    public Rotation2d getRotation() {
+        return gyro.getRotation2d();
+    }
+
+    public SwerveDriveWheelPositions getWheelPositions() {
+        return new SwerveDriveWheelPositions(new SwerveModulePosition[] {
+            modules.get(0).getModulePosition(),
+            modules.get(1).getModulePosition(),
+            modules.get(2).getModulePosition(),
+            modules.get(3).getModulePosition()
+        });
+    }
+
+    public void setSetpoint(ChassisSpeeds speeds) {
+        this.setPoint = speeds;
+    }
+
+    @Override
+    public void periodic() {
+        if (DriverStation.isTeleop()) {
+            SwerveModuleState[] states = DrivetrainConstants.SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(ChassisSpeeds.discretize(setPoint, Units.millisecondsToSeconds(20)));
+            SwerveDriveKinematics.desaturateWheelSpeeds(states, DrivetrainConstants.MAX_PHYSICAL_SPEED_M_S); //FIXME
+            for (int i = 0; i < modules.size(); i++) {
+                modules.get(i).setModuleState(states[i]);
+            }
+        }
+
+        if (DriverStation.isDisabled()) {
+            for (int i = 0; i < modules.size(); i++) {
+                modules.get(i).resetController();
+            }
+        }
+        log();
+    }
+
+    public void setStatesAuton(SwerveModuleState[] states) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, DrivetrainConstants.MAX_PHYSICAL_SPEED_M_S); //FIXME
+        for (int i = 0; i < modules.size(); i++) {
+            modules.get(i).setModuleState(states[i]);
+        }
+    }
+
+    public void setSpeedsAuton(ChassisSpeeds speeds) {
+        SwerveModuleState[] states = DrivetrainConstants.SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(ChassisSpeeds.discretize(speeds, Units.millisecondsToSeconds(20)));
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, DrivetrainConstants.MAX_PHYSICAL_SPEED_M_S); //FIXME
+        for (int i = 0; i < modules.size(); i++) {
+            modules.get(i).setModuleState(states[i]);
+        }
+    }
+
+    public ChassisSpeeds getVelocityVector() {
+        SwerveModuleState[] states = new SwerveModuleState[4];
+        for (int i = 0; i < modules.size(); i++) {
+            states[i] = modules.get(i).getModuleState();
+        }
+        ChassisSpeeds speeds = DrivetrainConstants.SWERVE_DRIVE_KINEMATICS.toChassisSpeeds(states);
+        return speeds;
+    }
+
+    public ChassisSpeeds getDriveSpeeds(CommandXboxController controller) {
+        double[] cv = {
+            -controller.getRightY() * DrivetrainConstants.MAX_TRANSLATION_SPEED_M_S_TELEOP, 
+            -controller.getRightX() * DrivetrainConstants.MAX_TRANSLATION_SPEED_M_S_TELEOP, 
+            -controller.getLeftX() * DrivetrainConstants.MAX_ROTATION_SPEED_RAD_S_TELEOP
+        }; //FIXME may need to negate
+
+        if (!MiscUtil.isBlue()) {
+            cv[0] = -cv[0];
+            cv[1] = -cv[1];
+        }
+
+        return ChassisSpeeds.fromFieldRelativeSpeeds(cv[0], cv[1], cv[2], this.getRotation());
+    }
+
+    public void log() {
+        SteelTalonsLogger.post("Drivetrain Setpoint X", setPoint.vxMetersPerSecond);
+        SteelTalonsLogger.post("Drivetrain Setpoint Y", setPoint.vyMetersPerSecond);
+        SteelTalonsLogger.post("Drivetrain Setpoint Theta", setPoint.omegaRadiansPerSecond);
+        SteelTalonsLogger.post("Drivetrain Velocity X", getVelocityVector().vxMetersPerSecond);
+        SteelTalonsLogger.post("Drivetrain Velocity Y", getVelocityVector().vyMetersPerSecond);
+        SteelTalonsLogger.post("Drivetrain Velocity Theta", getVelocityVector().omegaRadiansPerSecond);
+
+        modules.get(0).log("FRONT LEFT");
+        // modules.get(1).log("FRONT RIGHT");
+        // modules.get(2).log("BACK LEFT");
+        // modules.get(3).log("BACK RIGHT");
+    }
+
+    public Command getDriveCommand() {
+        return new DriveCommand(getInstance(), new CommandXboxController(0));
+    }
+
+
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/Swerve/SwerveModule.java
new file mode 100644
index 0000000..cc2d42b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Swerve/SwerveModule.java
@@ -0,0 +1,67 @@
+package frc.robot.subsystems.Swerve;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.util.STSmaxConfig;
+import frc.robot.util.SteelTalonsLogger;
+import frc.robot.util.SteelTalonsSparkMaxFlywheel;
+import frc.robot.util.SteelTalonsSparkMaxServo;
+
+public class SwerveModule {
+    // private TalonFX driveMotor;
+    private SteelTalonsSparkMaxFlywheel driveMotor;
+    private SteelTalonsSparkMaxServo steerMotor;
+    private CANcoder canCoder;
+
+    public SwerveModule(STSmaxConfig driveConfig, STSmaxConfig steerConfig, int canCoderID, double offset) {
+        DrivetrainConstants.configureMotors();
+
+        driveMotor = new SteelTalonsSparkMaxFlywheel(DrivetrainConstants.configureDriveNeo(driveConfig));
+    
+        steerMotor = new SteelTalonsSparkMaxServo(DrivetrainConstants.configureSteerNeo(steerConfig));
+
+        canCoder = new CANcoder(canCoderID);
+        DrivetrainConstants.configureCanCoder(canCoder, offset);
+
+        driveMotor.setPosition(0);
+        steerMotor.setPosition(canCoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI);
+    }
+
+    public SwerveModulePosition getModulePosition() {
+        return new SwerveModulePosition(driveMotor.getPosition(), new Rotation2d(steerMotor.getPosition()));
+    }
+
+    public SwerveModuleState getModuleState() {
+        return new SwerveModuleState(driveMotor.getVelocity(), new Rotation2d(steerMotor.getPosition()));
+    }
+
+    public void setModuleState(SwerveModuleState state) {
+        double velocitySetpoint = state.speedMetersPerSecond;
+        Rotation2d rotSetpoint = state.angle;
+        if (velocitySetpoint > DrivetrainConstants.THRESHOLD_STOPPING_M_S) {
+            steerMotor.setSetpoint(rotSetpoint.getRadians(), 0);
+            driveMotor.setSetpoint(velocitySetpoint, 0);
+        } else {
+            steerMotor.forceStop();
+            driveMotor.forceStop();
+        }
+    }
+
+    public void resetController() {
+        steerMotor.resetController();
+    } 
+
+    public void log(String name) {
+        SteelTalonsLogger.post(name + " velocity", getModuleState().speedMetersPerSecond);
+        SteelTalonsLogger.post(name + " position", getModulePosition().distanceMeters);
+        SteelTalonsLogger.post(name + " angle", getModuleState().angle.getRadians());
+        SteelTalonsLogger.post(name + " absolute angle", canCoder.getAbsolutePosition());
+        // steerMotor.log();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Wrist/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist/Wrist.java
new file mode 100644
index 0000000..fc53ba4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Wrist/Wrist.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.Wrist;
+
+public class Wrist {
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/Wrist/WristConstants.java b/src/main/java/frc/robot/subsystems/Wrist/WristConstants.java
new file mode 100644
index 0000000..e885f33
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Wrist/WristConstants.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.Wrist;
+
+public class WristConstants {
+    
+}
diff --git a/src/main/java/frc/robot/util/AutonUtil.java b/src/main/java/frc/robot/util/AutonUtil.java
new file mode 100644
index 0000000..8756013
--- /dev/null
+++ b/src/main/java/frc/robot/util/AutonUtil.java
@@ -0,0 +1,31 @@
+package frc.robot.util;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.Swerve.DrivetrainConstants;
+import frc.robot.subsystems.Swerve.SwerveDrivetrain;
+import frc.robot.util.Localization.SteelTalonsLocalization;
+
+public class AutonUtil extends SubsystemBase {
+    public AutonUtil() {
+        AutoBuilder.configureHolonomic(
+            SteelTalonsLocalization.getInstance()::getPose, 
+            SteelTalonsLocalization.getInstance()::resetPose, 
+            SwerveDrivetrain.getInstance()::getVelocityVector, 
+            SwerveDrivetrain.getInstance()::setSpeedsAuton, 
+            new HolonomicPathFollowerConfig(
+                DrivetrainConstants.MAX_PHYSICAL_SPEED_M_S, 
+                Math.hypot(DrivetrainConstants.TRACKWIDTH, DrivetrainConstants.WHEELBASE), 
+                new ReplanningConfig()
+            ), 
+            () -> {
+                return MiscUtil.isBlue();
+            },
+            SwerveDrivetrain.getInstance()
+        );
+
+    }
+}
diff --git a/src/main/java/frc/robot/util/Localization/ApriltagCam.java b/src/main/java/frc/robot/util/Localization/ApriltagCam.java
new file mode 100644
index 0000000..d86603a
--- /dev/null
+++ b/src/main/java/frc/robot/util/Localization/ApriltagCam.java
@@ -0,0 +1,29 @@
+package frc.robot.util.Localization;
+
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ApriltagCam extends SubsystemBase {
+
+    private PhotonCamera cam;
+    private PhotonPoseEstimator estimator;
+
+    public ApriltagCam(String name, Transform3d robotToCam, AprilTagFieldLayout fieldLayout) {
+        cam = new PhotonCamera(name);
+        estimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cam, robotToCam);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS);
+    }
+
+    public Optional<EstimatedRobotPose> getUpdate(Pose2d refPose) {
+        return estimator.update();
+    }
+}
diff --git a/src/main/java/frc/robot/util/Localization/LocalizationUtil.java b/src/main/java/frc/robot/util/Localization/LocalizationUtil.java
new file mode 100644
index 0000000..8d2624d
--- /dev/null
+++ b/src/main/java/frc/robot/util/Localization/LocalizationUtil.java
@@ -0,0 +1,24 @@
+package frc.robot.util.Localization;
+
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+public class LocalizationUtil {
+    public static Optional<SteelTalonsVisionMeasurement> findConfidence(Optional<EstimatedRobotPose> estimate, Pose2d refPose) {
+        if (estimate.isPresent()) {
+            double timestamp = estimate.get().timestampSeconds;
+            Pose2d pose = estimate.get().estimatedPose.toPose2d();
+            Matrix<N3, N1> confidence = VecBuilder.fill(0.0, 0.0, Double.MAX_VALUE); //FIXME
+            return Optional.of(new SteelTalonsVisionMeasurement(pose, confidence, timestamp));
+        } else {
+            return Optional.empty();
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/util/Localization/SteelTalonsLocalization.java b/src/main/java/frc/robot/util/Localization/SteelTalonsLocalization.java
new file mode 100644
index 0000000..89a61d9
--- /dev/null
+++ b/src/main/java/frc/robot/util/Localization/SteelTalonsLocalization.java
@@ -0,0 +1,89 @@
+package frc.robot.util.Localization;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.Optional;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+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.Transform3d;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.Swerve.DrivetrainConstants;
+import frc.robot.subsystems.Swerve.SwerveDrivetrain;
+
+public class SteelTalonsLocalization extends SubsystemBase {
+
+    private static SteelTalonsLocalization instance;
+    private AprilTagFieldLayout aprilTagFieldLayout;
+    private ArrayList<ApriltagCam> camList;
+
+    private final String leftCamName = "left";
+    private final String rightCamName = "right";
+    private final Transform3d leftRobotToCam = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0));
+    private final Transform3d rightRobotToCam = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0));
+
+    private SwerveDrivePoseEstimator poseEstimator;
+
+    public SteelTalonsLocalization() {
+        instance = this;
+        try {
+            aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
+        } catch (IOException ex) {
+            System.err.println("Couldn't load field");
+        }
+
+        // camList.add(new ApriltagCam(leftCamName, leftRobotToCam, aprilTagFieldLayout));
+        // camList.add(new ApriltagCam(rightCamName, rightRobotToCam, aprilTagFieldLayout));
+
+        poseEstimator = new SwerveDrivePoseEstimator(
+            DrivetrainConstants.SWERVE_DRIVE_KINEMATICS, 
+            SwerveDrivetrain.getInstance().getRotation(), 
+            SwerveDrivetrain.getInstance().getWheelPositions().positions, 
+            new Pose2d(), 
+            VecBuilder.fill(0.5, 0.5, 0), 
+            VecBuilder.fill(0.03, 0.03, Double.MAX_VALUE)
+        );
+    }
+
+    public static SteelTalonsLocalization getInstance() {
+        return instance;
+    }
+
+    @Override
+    public void periodic() {
+        SwerveDriveWheelPositions dtWheelPositions = SwerveDrivetrain.getInstance().getWheelPositions();
+        Rotation2d gyroAngle = SwerveDrivetrain.getInstance().getRotation();
+        poseEstimator.update(gyroAngle, dtWheelPositions);
+        Pose2d refPose = poseEstimator.getEstimatedPosition();
+
+        if (camList != null) {
+            for (ApriltagCam cam : camList) {
+                Optional<SteelTalonsVisionMeasurement> estimate = LocalizationUtil.findConfidence(cam.getUpdate(refPose), refPose);
+                if (estimate.isPresent()) {
+                    SteelTalonsVisionMeasurement m = estimate.get();
+                    poseEstimator.addVisionMeasurement(m.getPose(), m.getTimestamp(), m.getConfidence());
+                } 
+            }
+
+        }
+    }
+
+    public Pose2d getPose() {
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    public void resetPose(Pose2d newPose) {
+        poseEstimator.resetPosition(
+            SwerveDrivetrain.getInstance().getRotation(), 
+            SwerveDrivetrain.getInstance().getWheelPositions().positions, 
+            newPose
+        );
+    }
+
+}
diff --git a/src/main/java/frc/robot/util/Localization/SteelTalonsVisionMeasurement.java b/src/main/java/frc/robot/util/Localization/SteelTalonsVisionMeasurement.java
new file mode 100644
index 0000000..2504af1
--- /dev/null
+++ b/src/main/java/frc/robot/util/Localization/SteelTalonsVisionMeasurement.java
@@ -0,0 +1,30 @@
+package frc.robot.util.Localization;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+public class SteelTalonsVisionMeasurement {
+    private Pose2d pose;
+    private Matrix<N3, N1> confidence;
+    private double timestamp;
+
+    public SteelTalonsVisionMeasurement(Pose2d pose, Matrix<N3, N1> confidence, double timestamp) {
+        this.pose = pose;
+        this.confidence = confidence;
+        this.timestamp = timestamp;
+    }
+
+    public Pose2d getPose() {
+        return pose;
+    }
+
+    public Matrix<N3, N1> getConfidence() {
+        return confidence;
+    }
+
+    public double getTimestamp() {
+        return timestamp;
+    }
+}
diff --git a/src/main/java/frc/robot/util/MiscUtil.java b/src/main/java/frc/robot/util/MiscUtil.java
new file mode 100644
index 0000000..6e107a7
--- /dev/null
+++ b/src/main/java/frc/robot/util/MiscUtil.java
@@ -0,0 +1,81 @@
+package frc.robot.util;
+
+import java.util.Optional;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+
+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.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class MiscUtil {
+    private static final double fieldWidth = Units.feetToMeters(54);
+    private static final double fieldHeight = Units.feetToMeters(27);
+    private static final int MAX_SMAX_PERIODIC_FRAME_MS = 65534;
+
+    public static Pose2d flip(Pose2d pose) {
+        Translation2d trans = pose.getTranslation();
+        Rotation2d rot = pose.getRotation();
+        return new Pose2d(flip(trans), flip(rot));
+    }
+
+    public static Translation2d flip(Translation2d trans) {
+        double x = trans.getX();
+        x = (fieldWidth - x);
+        return new Translation2d(x, trans.getY());
+    }
+
+    public static Rotation2d flip(Rotation2d rot) {
+        double x = rot.getCos();
+        x *= -1;
+        return new Rotation2d(x, rot.getSin());
+    }
+
+    public static boolean isBlue() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if (alliance.isPresent()) {
+            return alliance.get().equals(DriverStation.Alliance.Blue);
+        } else {
+            return false;
+        }
+    }
+
+    public static Pose2d[] resetPose() { //blue, then red
+        return new Pose2d[]{
+            new Pose2d(),
+            new Pose2d(fieldWidth, 0, new Rotation2d(Math.PI))
+        };
+    }
+
+    public static void doPeriodicFrame(CANSparkMax... motor) {
+        for (CANSparkMax m : motor) {
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus1, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus2, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus3, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus4, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus5, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus6, MAX_SMAX_PERIODIC_FRAME_MS);
+        }
+    }
+
+    public static void doPeriodicFrame(int msZero, CANSparkMax... motor) {
+        for (CANSparkMax m : motor) {
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus0, msZero);
+        }
+        doPeriodicFrame(motor);
+    }
+
+    public static void doPeriodicFrameLess(CANSparkMax... motor) {
+        for (CANSparkMax m : motor) {
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus3, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus4, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus5, MAX_SMAX_PERIODIC_FRAME_MS);
+            m.setPeriodicFramePeriod(PeriodicFrame.kStatus6, MAX_SMAX_PERIODIC_FRAME_MS);
+        }
+    }
+
+}
diff --git a/src/main/java/frc/robot/util/STSmaxConfig.java b/src/main/java/frc/robot/util/STSmaxConfig.java
new file mode 100644
index 0000000..290b40f
--- /dev/null
+++ b/src/main/java/frc/robot/util/STSmaxConfig.java
@@ -0,0 +1,28 @@
+package frc.robot.util;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+public class STSmaxConfig {
+    public String name;
+    public int id;
+    public boolean inverted;
+    public double gearing;
+    public boolean isRotational;
+    public double finalDiameterMeters; //only useful for linear motors
+    public int currentLimit;
+    public double kP, kD, kI, kFF;
+    public double maxAccel, maxVel;
+    public IdleMode idleMode;
+    public STSmaxConfig() {
+        name = "NOT CONSTRUCUTED";
+        id = 0;
+        inverted = false;
+        gearing = 1.0;
+        isRotational = true;
+        finalDiameterMeters = 1.0 / Math.PI; //Circumference is 1
+        currentLimit = 40;
+        maxAccel = 0.0;
+        maxVel = 0.0;
+        idleMode = IdleMode.kBrake;
+    }
+}
diff --git a/src/main/java/frc/robot/util/SteelTalonsLogger.java b/src/main/java/frc/robot/util/SteelTalonsLogger.java
new file mode 100644
index 0000000..215bb8e
--- /dev/null
+++ b/src/main/java/frc/robot/util/SteelTalonsLogger.java
@@ -0,0 +1,34 @@
+package frc.robot.util;
+
+import java.util.HashMap;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+public class SteelTalonsLogger {
+    private static ShuffleboardTab mainTab;
+    private static HashMap<String, GenericEntry> widgetList;
+    public SteelTalonsLogger() {
+        mainTab = Shuffleboard.getTab("5427_Logger");
+        widgetList = new HashMap<String, GenericEntry>();
+    }
+
+    public static boolean post(String key, Object obj) {
+        if (!widgetList.containsKey(key)) {
+            widgetList.put(key, mainTab.add(key, obj).getEntry());
+            return widgetList.get(key).setValue(obj);
+        } else {
+            return widgetList.get(key).setValue(obj);
+        }
+    }
+
+    public static void postComplex(String key, Sendable sendable) {
+        try {
+            mainTab.add(key, sendable);
+        } catch (Exception e) {
+            return;
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/util/SteelTalonsSparkMaxFlywheel.java b/src/main/java/frc/robot/util/SteelTalonsSparkMaxFlywheel.java
new file mode 100644
index 0000000..dad072a
--- /dev/null
+++ b/src/main/java/frc/robot/util/SteelTalonsSparkMaxFlywheel.java
@@ -0,0 +1,99 @@
+package frc.robot.util;
+
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
+
+public class SteelTalonsSparkMaxFlywheel {
+
+    private CANSparkMax smax;
+    private RelativeEncoder smaxEnc;
+    private SparkPIDController smaxPID;
+    private STSmaxConfig config;
+    private double setPoint = 0;
+
+
+    public SteelTalonsSparkMaxFlywheel(STSmaxConfig config) {
+        config.isRotational = false;
+        this.config = config;
+        smax = new CANSparkMax(config.id, MotorType.kBrushless);
+        Timer.delay(0.15);
+        smax.setInverted(config.inverted);
+        smax.setSmartCurrentLimit(config.currentLimit);
+        smax.setIdleMode(config.idleMode);
+        smaxEnc = smax.getEncoder();
+        smaxEnc.setMeasurementPeriod(10);
+        double positionConv = (config.gearing * config.finalDiameterMeters * Math.PI);
+        //M - M/s
+        smaxEnc.setPositionConversionFactor(positionConv);
+        smaxEnc.setVelocityConversionFactor(positionConv / 60);
+        smaxEnc.setPosition(0);
+        smaxPID = smax.getPIDController();
+        smaxPID.setP(config.kP);
+        smaxPID.setD(config.kD);
+        smaxPID.setFF(config.kFF);
+        smaxPID.setI(config.kI);
+        smax.burnFlash();
+        Timer.delay(0.15);
+
+    }
+
+    public void setRaw(double percent) {
+        smax.setVoltage(percent * smax.getBusVoltage());
+    }
+
+    public CANSparkMax getSmax() {
+        return smax;
+    }
+
+    public void setSetpoint(double setPoint, double arbFF) {
+        this.setPoint = setPoint;
+        smaxPID.setReference(setPoint, ControlType.kVelocity, 0, arbFF);
+    }
+
+    public double getSetPoint() {
+        return setPoint;
+    }
+
+    public void forceStop() {
+        smax.setVoltage(0);
+    }
+
+    public double getPosition() {
+        return smaxEnc.getPosition();
+    }
+
+    public void setPosition(double pos) {
+        smaxEnc.setPosition(pos);
+    }
+
+    public double getVelocity() {
+        return smaxEnc.getVelocity();
+    }
+
+    public double getError() {
+        if (config.isRotational) {
+            return new Rotation2d(setPoint).minus(new Rotation2d(getPosition())).getRadians();
+        } else {
+            return setPoint - getPosition();
+        }
+    }
+
+    public void log() {
+        String name = config.name;
+        SteelTalonsLogger.post(name + ": Applied Output (%)", smax.getAppliedOutput());
+        SteelTalonsLogger.post(name + ": Output Current (A)", smax.getOutputCurrent());
+        SteelTalonsLogger.post(name + ": Temp (C)", smax.getMotorTemperature());
+        SteelTalonsLogger.post(name + ": Is Braked? (Bool)", smax.getIdleMode().equals(IdleMode.kBrake));
+        SteelTalonsLogger.post(name + ": Position (Meters)", getPosition());
+        SteelTalonsLogger.post(name + ": Velocity (Meters/s)", getVelocity());
+        SteelTalonsLogger.post(name + ": Setpoint (Meters)", getSetPoint());
+        SteelTalonsLogger.post(name + ": Error (Meters)", getError());
+    }
+}
diff --git a/src/main/java/frc/robot/util/SteelTalonsSparkMaxServo.java b/src/main/java/frc/robot/util/SteelTalonsSparkMaxServo.java
new file mode 100644
index 0000000..f36dfdd
--- /dev/null
+++ b/src/main/java/frc/robot/util/SteelTalonsSparkMaxServo.java
@@ -0,0 +1,106 @@
+package frc.robot.util;
+
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
+import com.revrobotics.SparkPIDController.AccelStrategy;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj.Timer;
+
+public class SteelTalonsSparkMaxServo {
+
+    private CANSparkMax smax;
+    private RelativeEncoder smaxEnc;
+    private SparkPIDController smaxPID;
+    private ProfiledPIDController smaxController;
+    private STSmaxConfig config;
+    private double setPoint = 0;
+
+    public SteelTalonsSparkMaxServo(STSmaxConfig config) {
+        this.config = config;
+        smax = new CANSparkMax(config.id, MotorType.kBrushless);
+        Timer.delay(0.15);
+        smax.setInverted(config.inverted);
+        smax.setSmartCurrentLimit(config.currentLimit);
+        smax.setIdleMode(config.idleMode);
+        smaxEnc = smax.getEncoder();
+        smaxEnc.setMeasurementPeriod(10);
+        double positionConv = config.isRotational ? (2 * Math.PI * config.gearing) : (config.gearing * config.finalDiameterMeters * Math.PI);
+        //Rotational subsystem: Rad - Rad/s --- Linear subsystem: M - M/s
+        smaxEnc.setPositionConversionFactor(positionConv);
+        smaxEnc.setVelocityConversionFactor(positionConv / 60.0);
+        smaxEnc.setPosition(0);
+        smaxController = new ProfiledPIDController(config.kP, config.kD, config.kI, 
+            new Constraints(config.maxVel, config.maxAccel)
+        );
+        if (config.isRotational) {
+            smaxController.enableContinuousInput(-Math.PI, Math.PI);
+        }
+        smax.burnFlash();
+        Timer.delay(0.15);
+    }
+
+    public void setRaw(double percent) {
+        smax.set(percent);
+    }
+
+    public CANSparkMax getSmax() {
+        return smax;
+    }
+
+    public void setSetpoint(double setPoint, double arbFF) {
+        this.setPoint = setPoint;
+        smax.setVoltage(smaxController.calculate(getPosition(), setPoint) + arbFF + config.kFF * setPoint);
+    }
+
+    public double getSetPoint() {
+        return setPoint;
+    }
+
+    public void forceStop() {
+        smaxController.reset(getPosition());
+        smax.setVoltage(0);
+    }
+
+    public double getPosition() {
+        return smaxEnc.getPosition();
+    }
+
+    public void setPosition(double pos) {
+        smaxEnc.setPosition(pos);
+    }
+
+    public double getVelocity() {
+        return smaxEnc.getVelocity();
+    }
+
+    public double getError() {
+        if (config.isRotational) {
+            return new Rotation2d(setPoint).minus(new Rotation2d(getPosition())).getRadians();
+        } else {
+            return setPoint - getPosition();
+        }
+    }
+
+    public void resetController() {
+        smaxController.reset(getPosition());
+    }
+
+    public void log() {
+        String name = config.name;
+        SteelTalonsLogger.post(name + ": Applied Output (%)", smax.getAppliedOutput());
+        SteelTalonsLogger.post(name + ": Output Current (A)", smax.getOutputCurrent());
+        SteelTalonsLogger.post(name + ": Temp (C)", smax.getMotorTemperature());
+        SteelTalonsLogger.post(name + ": Is Braked? (Bool)", smax.getIdleMode().equals(IdleMode.kBrake));
+        SteelTalonsLogger.post(name + ": Position (rad or Meters)", getPosition());
+        SteelTalonsLogger.post(name + ": Velocity (rad/s or Meters/s)", getVelocity());
+        SteelTalonsLogger.post(name + ": Setpoint (rad or Meters)", getSetPoint());
+        SteelTalonsLogger.post(name + ": Error (rad or Meters)", getError());
+    }
+}
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 0000000..3c74146
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.1.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.1.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.1.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
new file mode 100644
index 0000000..69a4079
--- /dev/null
+++ b/vendordeps/Phoenix6.json
@@ -0,0 +1,339 @@
+{
+    "fileName": "Phoenix6.json",
+    "name": "CTRE-Phoenix (v6)",
+    "version": "24.1.0",
+    "frcYear": 2024,
+    "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json",
+    "conflictsWith": [
+        {
+            "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
+            "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
+            "offlineFileName": "Phoenix6And5.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "24.1.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "24.1.0",
+            "libName": "CTRE_Phoenix6_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.1.0",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "24.1.0",
+            "libName": "CTRE_Phoenix6_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "24.1.0",
+            "libName": "CTRE_PhoenixTools_Sim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "24.1.0",
+            "libName": "CTRE_SimTalonSRX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.1.0",
+            "libName": "CTRE_SimTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.1.0",
+            "libName": "CTRE_SimVictorSPX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "24.1.0",
+            "libName": "CTRE_SimPigeonIMU",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "24.1.0",
+            "libName": "CTRE_SimCANCoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "24.1.0",
+            "libName": "CTRE_SimProTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "24.1.0",
+            "libName": "CTRE_SimProCANcoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "24.1.0",
+            "libName": "CTRE_SimProPigeon2",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
new file mode 100644
index 0000000..0f3520e
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,74 @@
+{
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2024.2.0",
+    "frcYear": "2024",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2024.2.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2024.2.0",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2024.2.0",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2024.2.0",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 0000000..67bf389
--- /dev/null
+++ b/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2024",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ]
+}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..c940b75
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,42 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.1.1-beta-3.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "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": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2024.1.1-beta-3.2",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2024.1.1-beta-3.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2024.1.1-beta-3.2"
+        }
+    ]
+}
\ No newline at end of file