diff --git a/.vscode/settings.json b/.vscode/settings.json
index d2f7034..0df8b28 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -57,5 +57,5 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }
diff --git a/Test/.gitattributes b/Test/.gitattributes
new file mode 100644
index 0000000..f91f646
--- /dev/null
+++ b/Test/.gitattributes
@@ -0,0 +1,12 @@
+#
+# https://help.github.com/articles/dealing-with-line-endings/
+#
+# Linux start script should use lf
+/gradlew        text eol=lf
+
+# These are Windows script files and should use crlf
+*.bat           text eol=crlf
+
+# Binary files should be left untouched
+*.jar           binary
+
diff --git a/Test/.gitignore b/Test/.gitignore
new file mode 100644
index 0000000..1b6985c
--- /dev/null
+++ b/Test/.gitignore
@@ -0,0 +1,5 @@
+# Ignore Gradle project-specific cache directory
+.gradle
+
+# Ignore Gradle build output directory
+build
diff --git a/Test/app/build.gradle b/Test/app/build.gradle
new file mode 100644
index 0000000..d4f8fe4
--- /dev/null
+++ b/Test/app/build.gradle
@@ -0,0 +1,43 @@
+/*
+ * This file was generated by the Gradle 'init' task.
+ *
+ * This generated file contains a sample Java application project to get you started.
+ * For more details on building Java & JVM projects, please refer to https://docs.gradle.org/8.11/userguide/building_java_projects.html in the Gradle documentation.
+ */
+
+plugins {
+    // Apply the application plugin to add support for building a CLI application in Java.
+    id 'application'
+}
+
+repositories {
+    // Use Maven Central for resolving dependencies.
+    mavenCentral()
+}
+
+dependencies {
+    // Use JUnit Jupiter for testing.
+    testImplementation libs.junit.jupiter
+
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    // This dependency is used by the application.
+    implementation libs.guava
+}
+
+// Apply a specific Java toolchain to ease working on different environments.
+java {
+    toolchain {
+        languageVersion = JavaLanguageVersion.of(21)
+    }
+}
+
+application {
+    // Define the main class for the application.
+    mainClass = 'test.App'
+}
+
+tasks.named('test') {
+    // Use JUnit Platform for unit tests.
+    useJUnitPlatform()
+}
diff --git a/Test/app/src/main/java/test/App.java b/Test/app/src/main/java/test/App.java
new file mode 100644
index 0000000..507fe91
--- /dev/null
+++ b/Test/app/src/main/java/test/App.java
@@ -0,0 +1,14 @@
+/*
+ * This source file was generated by the Gradle 'init' task
+ */
+package test;
+
+public class App {
+    public String getGreeting() {
+        return "Hello World!";
+    }
+
+    public static void main(String[] args) {
+        System.out.println(new App().getGreeting());
+    }
+}
diff --git a/Test/app/src/test/java/test/AppTest.java b/Test/app/src/test/java/test/AppTest.java
new file mode 100644
index 0000000..7c490f3
--- /dev/null
+++ b/Test/app/src/test/java/test/AppTest.java
@@ -0,0 +1,14 @@
+/*
+ * This source file was generated by the Gradle 'init' task
+ */
+package test;
+
+import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.*;
+
+class AppTest {
+    @Test void appHasAGreeting() {
+        App classUnderTest = new App();
+        assertNotNull(classUnderTest.getGreeting(), "app should have a greeting");
+    }
+}
diff --git a/Test/gradle.properties b/Test/gradle.properties
new file mode 100644
index 0000000..377538c
--- /dev/null
+++ b/Test/gradle.properties
@@ -0,0 +1,5 @@
+# This file was generated by the Gradle 'init' task.
+# https://docs.gradle.org/current/userguide/build_environment.html#sec:gradle_configuration_properties
+
+org.gradle.configuration-cache=true
+
diff --git a/Test/gradle/libs.versions.toml b/Test/gradle/libs.versions.toml
new file mode 100644
index 0000000..239e149
--- /dev/null
+++ b/Test/gradle/libs.versions.toml
@@ -0,0 +1,10 @@
+# This file was generated by the Gradle 'init' task.
+# https://docs.gradle.org/current/userguide/platforms.html#sub::toml-dependencies-format
+
+[versions]
+guava = "33.2.1-jre"
+junit-jupiter = "5.10.3"
+
+[libraries]
+guava = { module = "com.google.guava:guava", version.ref = "guava" }
+junit-jupiter = { module = "org.junit.jupiter:junit-jupiter", version.ref = "junit-jupiter" }
diff --git a/Test/gradle/wrapper/gradle-wrapper.jar b/Test/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..a4b76b9
Binary files /dev/null and b/Test/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/Test/gradle/wrapper/gradle-wrapper.properties b/Test/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 0000000..94113f2
--- /dev/null
+++ b/Test/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
diff --git a/Test/gradlew b/Test/gradlew
new file mode 100755
index 0000000..f5feea6
--- /dev/null
+++ b/Test/gradlew
@@ -0,0 +1,252 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+# This is normally unused
+# shellcheck disable=SC2034
+APP_BASE_NAME=${0##*/}
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/Test/gradlew.bat b/Test/gradlew.bat
new file mode 100644
index 0000000..9d21a21
--- /dev/null
+++ b/Test/gradlew.bat
@@ -0,0 +1,94 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/Test/settings.gradle b/Test/settings.gradle
new file mode 100644
index 0000000..d967ff8
--- /dev/null
+++ b/Test/settings.gradle
@@ -0,0 +1,14 @@
+/*
+ * This file was generated by the Gradle 'init' task.
+ *
+ * The settings file is used to specify which projects to include in your build.
+ * For more detailed information on multi-project builds, please refer to https://docs.gradle.org/8.11/userguide/multi_project_builds.html in the Gradle documentation.
+ */
+
+plugins {
+    // Apply the foojay-resolver plugin to allow automatic download of JDKs
+    id 'org.gradle.toolchains.foojay-resolver-convention' version '0.8.0'
+}
+
+rootProject.name = 'Test'
+include('app')
diff --git a/build.gradle b/build.gradle
index 11d95e6..b25fd8e 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
 }
 
 java {
@@ -101,4 +101,4 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
-}
+}
\ No newline at end of file
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..73cc713
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/src/main/deploy/Choreo paths.chor b/src/main/deploy/Choreo paths.chor
new file mode 100644
index 0000000..7d54267
--- /dev/null
+++ b/src/main/deploy/Choreo paths.chor	
@@ -0,0 +1,78 @@
+{
+ "name":"Choreo paths",
+ "version":1,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11 in",
+    "val":0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11 in",
+    "val":-0.2794
+   },
+   "y":{
+    "exp":"-11 in",
+    "val":-0.2794
+   }
+  },
+  "mass":{
+   "exp":"150 lbs",
+   "val":68.0388555
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.5",
+   "val":6.5
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"1.2 N * m",
+   "val":1.2
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "side":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "back":{
+    "exp":"16 in",
+    "val":0.4064
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[]
+}
diff --git a/src/main/deploy/Diagonal.traj b/src/main/deploy/Diagonal.traj
new file mode 100644
index 0000000..7577be3
--- /dev/null
+++ b/src/main/deploy/Diagonal.traj
@@ -0,0 +1,61 @@
+{
+ "name":"Diagonal",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.5299232006073, "y":6.882591247558594, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.022549152374268, "y":5.633831024169922, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.5299232006073 m", "val":2.5299232006073}, "y":{"exp":"6.882591247558594 m", "val":6.882591247558594}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.022549152374268 m", "val":6.022549152374268}, "y":{"exp":"5.633831024169922 m", "val":5.633831024169922}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.30082],
+  "samples":[
+    {"t":0.0, "x":2.52992, "y":6.88259, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.49045, "ay":-3.04275, "alpha":0.0, "fx":[144.42017,144.42017,144.42017,144.42017], "fy":[-51.75637,-51.75637,-51.75637,-51.75637]},
+    {"t":0.05003, "x":2.54055, "y":6.87878, "heading":0.0, "vx":0.42479, "vy":-0.15223, "omega":0.0, "ax":8.48988, "ay":-3.04188, "alpha":-0.00001, "fx":[144.41034,144.41036,144.41037,144.41033], "fy":[-51.74149,-51.74144,-51.74141,-51.74152]},
+    {"t":0.10006, "x":2.57243, "y":6.86736, "heading":0.0, "vx":0.84955, "vy":-0.30442, "omega":0.0, "ax":8.4891, "ay":-3.04094, "alpha":-0.00002, "fx":[144.39709,144.39713,144.39716,144.39707], "fy":[-51.72566,-51.72554,-51.72548,-51.72572]},
+    {"t":0.15009, "x":2.62556, "y":6.84832, "heading":0.0, "vx":1.27427, "vy":-0.45657, "omega":0.0, "ax":8.48803, "ay":-3.03993, "alpha":-0.00003, "fx":[144.37899,144.37906,144.3791,144.37895], "fy":[-51.70842,-51.70823,-51.70812,-51.70853]},
+    {"t":0.20013, "x":2.69993, "y":6.82168, "heading":0.0, "vx":1.69894, "vy":-0.60866, "omega":0.0, "ax":8.48655, "ay":-3.03878, "alpha":-0.00005, "fx":[144.35368,144.35378,144.35384,144.35362], "fy":[-51.68898,-51.6887,-51.68853,-51.68915]},
+    {"t":0.25016, "x":2.79556, "y":6.78742, "heading":0.0, "vx":2.12353, "vy":-0.76069, "omega":-0.00001, "ax":8.48439, "ay":-3.03742, "alpha":-0.00007, "fx":[144.31692,144.31707,144.31715,144.31684], "fy":[-51.66589,-51.66548,-51.66525,-51.66612]},
+    {"t":0.30019, "x":2.91242, "y":6.74556, "heading":0.0, "vx":2.54802, "vy":-0.91266, "omega":-0.00001, "ax":8.48107, "ay":-3.03567, "alpha":-0.00009, "fx":[144.26039,144.2606,144.26071,144.26028], "fy":[-51.63618,-51.63562,-51.63531,-51.6365]},
+    {"t":0.35022, "x":3.05051, "y":6.6961, "heading":0.0, "vx":2.97234, "vy":-1.06454, "omega":-0.00001, "ax":8.47545, "ay":-3.03312, "alpha":-0.00012, "fx":[144.16492,144.16519,144.16534,144.16477], "fy":[-51.59296,-51.5922,-51.59178,-51.59338]},
+    {"t":0.40025, "x":3.20983, "y":6.63904, "heading":0.0, "vx":3.39638, "vy":-1.21629, "omega":-0.00002, "ax":8.46426, "ay":-3.02857, "alpha":-0.00016, "fx":[143.97448,143.97481,143.97502,143.97427], "fy":[-51.51559,-51.51462,-51.51408,-51.51613]},
+    {"t":0.45028, "x":3.39035, "y":6.5744, "heading":0.0, "vx":3.81986, "vy":-1.36781, "omega":-0.00003, "ax":8.43195, "ay":-3.01618, "alpha":-0.00018, "fx":[143.4249,143.42524,143.42549,143.42465], "fy":[-51.30499,-51.3039,-51.30331,-51.30558]},
+    {"t":0.50031, "x":3.59202, "y":6.50219, "heading":0.0, "vx":4.24172, "vy":-1.51872, "omega":-0.00004, "ax":7.55274, "ay":-2.68454, "alpha":0.00074, "fx":[128.47508,128.46474,128.46867,128.47114], "fy":[-45.6607,-45.66569,-45.67254,-45.65386]},
+    {"t":0.55035, "x":3.81369, "y":6.42285, "heading":-0.00001, "vx":4.61959, "vy":-1.65303, "omega":0.0, "ax":0.0115, "ay":0.01483, "alpha":-0.00001, "fx":[0.19561,0.19555,0.1956,0.19555], "fy":[0.25219,0.25228,0.25221,0.25226]},
+    {"t":0.60038, "x":4.04483, "y":6.34016, "heading":-0.00001, "vx":4.62017, "vy":-1.65229, "omega":0.0, "ax":0.00297, "ay":0.00832, "alpha":0.0, "fx":[0.05052,0.05052,0.05052,0.05052], "fy":[0.14159,0.14159,0.14159,0.14159]},
+    {"t":0.65041, "x":4.27599, "y":6.25751, "heading":-0.00001, "vx":4.62032, "vy":-1.65187, "omega":0.0, "ax":0.00259, "ay":0.00726, "alpha":0.0, "fx":[0.04405,0.04405,0.04405,0.04405], "fy":[0.12345,0.12345,0.12345,0.12345]},
+    {"t":0.70044, "x":4.50715, "y":6.17487, "heading":-0.00001, "vx":4.62045, "vy":-1.65151, "omega":0.0, "ax":0.00002, "ay":0.01743, "alpha":0.0, "fx":[0.00035,0.00038,0.00036,0.00036], "fy":[0.29655,0.29656,0.29656,0.29655]},
+    {"t":0.75047, "x":4.73832, "y":6.09227, "heading":-0.00001, "vx":4.62045, "vy":-1.65063, "omega":0.0, "ax":-7.54205, "ay":2.71474, "alpha":0.00009, "fx":[-128.2882,-128.288,-128.28833,-128.28787], "fy":[46.17722,46.17655,46.17652,46.17724]},
+    {"t":0.8005, "x":4.96005, "y":6.01308, "heading":-0.00001, "vx":4.24311, "vy":-1.51481, "omega":0.0, "ax":-8.43326, "ay":3.01245, "alpha":0.0001, "fx":[-143.44716,-143.44737,-143.44749,-143.44703], "fy":[51.24125,51.24065,51.24032,51.24159]},
+    {"t":0.85053, "x":5.16178, "y":5.94106, "heading":-0.00001, "vx":3.82118, "vy":-1.36409, "omega":0.00001, "ax":-8.46607, "ay":3.02347, "alpha":0.00006, "fx":[-144.00528,-144.00542,-144.00549,-144.00521], "fy":[51.42851,51.42813,51.42792,51.42871]},
+    {"t":0.90057, "x":5.34236, "y":5.8766, "heading":-0.00001, "vx":3.39761, "vy":-1.21283, "omega":0.00001, "ax":-8.47755, "ay":3.02721, "alpha":0.00003, "fx":[-144.20073,-144.2008,-144.20084,-144.20069], "fy":[51.49208,51.49189,51.49178,51.49219]},
+    {"t":0.9506, "x":5.50174, "y":5.81971, "heading":0.0, "vx":2.97347, "vy":-1.06137, "omega":0.00001, "ax":-8.48343, "ay":3.02903, "alpha":0.00001, "fx":[-144.30068,-144.3007,-144.30071,-144.30067], "fy":[51.52288,51.52282,51.52279,51.52291]},
+    {"t":1.00063, "x":5.63989, "y":5.7704, "heading":0.0, "vx":2.54903, "vy":-0.90982, "omega":0.00001, "ax":-8.48701, "ay":3.03005, "alpha":-0.00001, "fx":[-144.36166,-144.36164,-144.36163,-144.36167], "fy":[51.54022,51.54026,51.54029,51.54019]},
+    {"t":1.05066, "x":5.7568, "y":5.72867, "heading":0.0, "vx":2.12441, "vy":-0.75823, "omega":0.00001, "ax":-8.48944, "ay":3.03067, "alpha":-0.00002, "fx":[-144.40293,-144.40288,-144.40286,-144.40295], "fy":[51.55073,51.55084,51.55091,51.55066]},
+    {"t":1.10069, "x":5.85246, "y":5.69453, "heading":0.0, "vx":1.69967, "vy":-0.6066, "omega":0.00001, "ax":-8.4912, "ay":3.03106, "alpha":-0.00003, "fx":[-144.43286,-144.4328,-144.43276,-144.43289], "fy":[51.55729,51.55747,51.55756,51.5572]},
+    {"t":1.15072, "x":5.92687, "y":5.66797, "heading":0.0, "vx":1.27485, "vy":-0.45495, "omega":0.00001, "ax":-8.49254, "ay":3.0313, "alpha":-0.00004, "fx":[-144.45567,-144.45559,-144.45555,-144.45571], "fy":[51.56138,51.5616,51.56172,51.56126]},
+    {"t":1.20075, "x":5.98002, "y":5.649, "heading":0.0, "vx":0.84995, "vy":-0.30329, "omega":0.00001, "ax":-8.4936, "ay":3.03144, "alpha":-0.00004, "fx":[-144.47372,-144.47363,-144.47358,-144.47377], "fy":[51.56382,51.56407,51.56421,51.56368]},
+    {"t":1.25079, "x":6.01192, "y":5.63762, "heading":0.0, "vx":0.42501, "vy":-0.15162, "omega":0.00001, "ax":-8.49481, "ay":3.03055, "alpha":-0.00016, "fx":[-144.49439,-144.49322,-144.49442,-144.49479], "fy":[51.54834,51.55162,51.54826,51.54721]},
+    {"t":1.30082, "x":6.02255, "y":5.63383, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/move in the x direction.traj b/src/main/deploy/move in the x direction.traj
new file mode 100644
index 0000000..34e687a
--- /dev/null
+++ b/src/main/deploy/move in the x direction.traj	
@@ -0,0 +1,58 @@
+{
+ "name":"move in the x direction",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.354316234588623, "y":6.1216278076171875, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.607905387878418, "y":6.102116107940674, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.354316234588623 m", "val":2.354316234588623}, "y":{"exp":"6.1216278076171875 m", "val":6.1216278076171875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.607905387878418 m", "val":6.607905387878418}, "y":{"exp":"6.102116107940674 m", "val":6.102116107940674}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.86742],
+  "samples":[
+    {"t":0.0, "x":2.35432, "y":6.12163, "heading":0.0, "vx":4.89743, "vy":-0.02247, "omega":0.0, "ax":0.21753, "ay":-0.001, "alpha":0.0, "fx":[3.70006,3.70006,3.70006,3.70006], "fy":[-0.01697,-0.01697,-0.01697,-0.01697]},
+    {"t":0.02991, "x":2.5009, "y":6.12096, "heading":0.0, "vx":4.90394, "vy":-0.02249, "omega":0.0, "ax":0.00026, "ay":0.0, "alpha":0.0, "fx":[0.00436,0.00436,0.00436,0.00436], "fy":[-0.00002,-0.00002,-0.00002,-0.00002]},
+    {"t":0.05982, "x":2.64758, "y":6.12028, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.08973, "x":2.79427, "y":6.11961, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.11964, "x":2.94095, "y":6.11894, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.14956, "x":3.08763, "y":6.11826, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.17947, "x":3.23431, "y":6.11759, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.20938, "x":3.38099, "y":6.11692, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.23929, "x":3.52768, "y":6.11625, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.2692, "x":3.67436, "y":6.11557, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.29911, "x":3.82104, "y":6.1149, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.32902, "x":3.96772, "y":6.11423, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.35893, "x":4.11441, "y":6.11355, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.38884, "x":4.26109, "y":6.11288, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.41875, "x":4.40777, "y":6.11221, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44867, "x":4.55445, "y":6.11154, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.47858, "x":4.70113, "y":6.11086, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.50849, "x":4.84782, "y":6.11019, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.5384, "x":4.9945, "y":6.10952, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.56831, "x":5.14118, "y":6.10884, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.59822, "x":5.28786, "y":6.10817, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.62813, "x":5.43455, "y":6.1075, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.65804, "x":5.58123, "y":6.10683, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.68795, "x":5.72791, "y":6.10615, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.71787, "x":5.87459, "y":6.10548, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.74778, "x":6.02127, "y":6.10481, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.77769, "x":6.16796, "y":6.10413, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.8076, "x":6.31464, "y":6.10346, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":-0.00026, "ay":0.0, "alpha":0.0, "fx":[-0.00436,-0.00436,-0.00436,-0.00436], "fy":[0.00002,0.00002,0.00002,0.00002]},
+    {"t":0.83751, "x":6.46132, "y":6.10279, "heading":0.0, "vx":4.90394, "vy":-0.02249, "omega":0.0, "ax":-0.21753, "ay":0.001, "alpha":0.0, "fx":[-3.70006,-3.70006,-3.70006,-3.70006], "fy":[0.01697,0.01697,0.01697,0.01697]},
+    {"t":0.86742, "x":6.60791, "y":6.10212, "heading":0.0, "vx":4.89743, "vy":-0.02247, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/move in the y direction.traj b/src/main/deploy/move in the y direction.traj
new file mode 100644
index 0000000..78a01e5
--- /dev/null
+++ b/src/main/deploy/move in the y direction.traj	
@@ -0,0 +1,47 @@
+{
+ "name":"move in the y direction",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.7250418663024902, "y":7.409411907196045, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.7250418663024902, "y":5.633831024169922, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.7250418663024902 m", "val":2.7250418663024902}, "y":{"exp":"7.409411907196045 m", "val":7.409411907196045}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.7250418663024902 m", "val":2.7250418663024902}, "y":{"exp":"5.633831024169922 m", "val":5.633831024169922}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.36234],
+  "samples":[
+    {"t":0.0, "x":2.72504, "y":7.40941, "heading":0.0, "vx":0.0, "vy":-4.89133, "omega":0.0, "ax":0.0, "ay":-0.46895, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-7.97677,-7.97677,-7.97677,-7.97677]},
+    {"t":0.02013, "x":2.72504, "y":7.31085, "heading":0.0, "vx":0.0, "vy":-4.90077, "omega":0.0, "ax":0.0, "ay":-0.00268, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.04558,-0.04558,-0.04558,-0.04558]},
+    {"t":0.04026, "x":2.72504, "y":7.2122, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":-0.00002, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00026,-0.00026,-0.00026,-0.00026]},
+    {"t":0.06039, "x":2.72504, "y":7.11355, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.08052, "x":2.72504, "y":7.01489, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.10065, "x":2.72504, "y":6.91624, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.12078, "x":2.72504, "y":6.81758, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.14091, "x":2.72504, "y":6.71893, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.16104, "x":2.72504, "y":6.62028, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.18117, "x":2.72504, "y":6.52162, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.2013, "x":2.72504, "y":6.42297, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.22143, "x":2.72504, "y":6.32431, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.24156, "x":2.72504, "y":6.22566, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.26169, "x":2.72504, "y":6.127, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.28182, "x":2.72504, "y":6.02835, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.30195, "x":2.72504, "y":5.9297, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.00002, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00026,0.00026,0.00026,0.00026]},
+    {"t":0.32208, "x":2.72504, "y":5.83104, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.00268, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.04558,0.04558,0.04558,0.04558]},
+    {"t":0.34221, "x":2.72504, "y":5.73239, "heading":0.0, "vx":0.0, "vy":-4.90077, "omega":0.0, "ax":0.0, "ay":0.46895, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[7.97677,7.97677,7.97677,7.97677]},
+    {"t":0.36234, "x":2.72504, "y":5.63383, "heading":0.0, "vx":0.0, "vy":-4.89133, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/pathplanner/autos/90 Auto.auto b/src/main/deploy/pathplanner/autos/90 Auto.auto
new file mode 100644
index 0000000..28dcc7f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/90 Auto.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Straight Line Rotate 90"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Testing",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto
new file mode 100644
index 0000000..151b9b3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto	
@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom to CD"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle CD"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle CD"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle CD"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear Low Alage"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CD to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Algae Intake",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Hot Reload.auto b/src/main/deploy/pathplanner/autos/Hot Reload.auto
new file mode 100644
index 0000000..b8296ca
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Hot Reload.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": null
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Testing",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Piecewise Rotation Test.auto b/src/main/deploy/pathplanner/autos/Piecewise Rotation Test.auto
new file mode 100644
index 0000000..5561849
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Piecewise Rotation Test.auto	
@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "G - J"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J - L"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "L - B"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "B - D"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "D - E"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "E - G"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Testing",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Slide and Drop.auto b/src/main/deploy/pathplanner/autos/Slide and Drop.auto
new file mode 100644
index 0000000..f8d9b2e
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Slide and Drop.auto	
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Middle to HG"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L1"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Slide and Score.auto b/src/main/deploy/pathplanner/autos/Slide and Score.auto
new file mode 100644
index 0000000..ca2226e
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Slide and Score.auto	
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Middle to HG"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto
new file mode 100644
index 0000000..3fbe6c3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto	
@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Middle to HG"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear Low Alage"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Face Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score Barge"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Return Home"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Algae Intake",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Straight Test.auto b/src/main/deploy/pathplanner/autos/Straight Test.auto
new file mode 100644
index 0000000..2c0acd5
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Straight Test.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Straight Test"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Testing",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto
new file mode 100644
index 0000000..9cdcd5a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto	
@@ -0,0 +1,115 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear Low Alage"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LK to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to IJ"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Barge to IJ"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear High Algae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "IJ to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Face Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score Barge"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Algae Intake",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto
new file mode 100644
index 0000000..cc63167
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto	
@@ -0,0 +1,91 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear Low Alage"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LK to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score Barge"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Barge to Station to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Algae Intake",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto
new file mode 100644
index 0000000..34bc30e
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto	
@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LK to JI"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle JI"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Algae Intake",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto
new file mode 100644
index 0000000..b0abb49
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto	
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L3"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto
new file mode 100644
index 0000000..859dc4c
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto	
@@ -0,0 +1,127 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate LK"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate LK"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Rotate to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Clear Low Alage"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "rotate LK"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "rotate LK"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "score R-L2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "rotate LK"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Top Ring Rotation.auto b/src/main/deploy/pathplanner/autos/Top Ring Rotation.auto
new file mode 100644
index 0000000..5c236be
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Top Ring Rotation.auto	
@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top to IJ"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle JI"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "IJ to LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cycle LK"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score R-L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LK to AB"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Score L-L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Triangle.auto b/src/main/deploy/pathplanner/autos/Triangle.auto
new file mode 100644
index 0000000..57cb1e9
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Triangle.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Triangle"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Testing",
+  "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..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"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],[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,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,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,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,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,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,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,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,true,true],[true,true,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,true,true,true,true,true,true,true,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,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,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,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,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,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/AB to Barge.path b/src/main/deploy/pathplanner/paths/AB to Barge.path
new file mode 100644
index 0000000..2a5b9cd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/AB to Barge.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.8672697368421054,
+        "y": 4.111595394736842
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8961348684210524,
+        "y": 6.007072368421052
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.524177631578947,
+        "y": 5.583717105263157
+      },
+      "prevControl": {
+        "x": 4.753125,
+        "y": 5.622203947368421
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Automatic Rotation Test.path b/src/main/deploy/pathplanner/paths/Automatic Rotation Test.path
new file mode 100644
index 0000000..8c98e8d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Automatic Rotation Test.path	
@@ -0,0 +1,134 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.302045454545453,
+        "y": 4.000071022727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.222272727272728,
+        "y": 4.747940340909091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.294914772727273,
+        "y": 5.356207386363637
+      },
+      "prevControl": {
+        "x": 5.721361536531213,
+        "y": 5.047401109126301
+      },
+      "nextControl": {
+        "x": 4.716562499999999,
+        "y": 5.775014204545455
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3903409090909093,
+        "y": 5.356207386363637
+      },
+      "prevControl": {
+        "x": 3.859005681818181,
+        "y": 5.545667613636363
+      },
+      "nextControl": {
+        "x": 2.921676136363638,
+        "y": 5.16674715909091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.7421875,
+        "y": 4.089815340909091
+      },
+      "prevControl": {
+        "x": 2.7920454545454545,
+        "y": 4.6681676136363635
+      },
+      "nextControl": {
+        "x": 2.6915775637307973,
+        "y": 3.5027400801863386
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5299431818181817,
+        "y": 2.65390625
+      },
+      "prevControl": {
+        "x": 2.9864914772727267,
+        "y": 3.0378125
+      },
+      "nextControl": {
+        "x": 4.073394886363637,
+        "y": 2.27
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.504318181818181,
+        "y": 2.65390625
+      },
+      "prevControl": {
+        "x": 5.002692388078105,
+        "y": 2.3916927669085957
+      },
+      "nextControl": {
+        "x": 5.943068181818181,
+        "y": 2.8832528409090914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.302045454545453,
+        "y": 4.000071022727272
+      },
+      "prevControl": {
+        "x": 6.212301136363636,
+        "y": 3.411747159090909
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B - D.path b/src/main/deploy/pathplanner/paths/B - D.path
new file mode 100644
index 0000000..328627d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/B - D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7891761363636363,
+        "y": 3.2421590909090914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.549886363636363,
+        "y": 2.4843892045454536
+      },
+      "prevControl": {
+        "x": 2.8319318181818174,
+        "y": 3.0328267045454544
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Barge to AB.path b/src/main/deploy/pathplanner/paths/Barge to AB.path
new file mode 100644
index 0000000..89b4825
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Barge to AB.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.553042763157894,
+        "y": 5.2195419520547945
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.377878289473684,
+        "y": 6.122532894736842
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0115953947368417,
+        "y": 3.9865131578947364
+      },
+      "prevControl": {
+        "x": 2.3958059210526317,
+        "y": 5.6703125
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Barge to HG.path b/src/main/deploy/pathplanner/paths/Barge to HG.path
new file mode 100644
index 0000000..962faeb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Barge to HG.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.553042763157894,
+        "y": 5.2195419520547945
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.177092905565743,
+        "y": 4.92330746482436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.955838815789473,
+        "y": 4.005756578947368
+      },
+      "prevControl": {
+        "x": 6.831269269372734,
+        "y": 4.6711797130262935
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Barge to IJ.path b/src/main/deploy/pathplanner/paths/Barge to IJ.path
new file mode 100644
index 0000000..78762a7
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Barge to IJ.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.6,
+        "y": 5.7
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.301390224789032,
+        "y": 5.638897810070038
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.19888698630137,
+        "y": 5.2195419520547945
+      },
+      "prevControl": {
+        "x": 5.557876031958477,
+        "y": 5.288158179669997
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path
new file mode 100644
index 0000000..8b3ed5c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 2.75
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.958476027397261,
+        "y": 1.8537885273972599
+      },
+      "isLocked": false,
+      "linkedName": "CD"
+    },
+    {
+      "anchor": {
+        "x": 7.557919520547944,
+        "y": 3.9123073630136984
+      },
+      "prevControl": {
+        "x": 5.168835616438354,
+        "y": 1.8838398972602746
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path
new file mode 100644
index 0000000..21a4666
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path	
@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.6,
+        "y": 5.7
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.820069045466418,
+        "y": 5.8536552289261135
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.7649715909090908,
+        "y": 7.220894886363636
+      },
+      "prevControl": {
+        "x": 1.9756273811587002,
+        "y": 7.086268967483531
+      },
+      "nextControl": {
+        "x": 1.5543158006594815,
+        "y": 7.355520805243741
+      },
+      "isLocked": false,
+      "linkedName": "Top Coral Station"
+    },
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 2.761554446829292,
+        "y": 6.179664311148375
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LK"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Bottom to CD.path b/src/main/deploy/pathplanner/paths/Bottom to CD.path
new file mode 100644
index 0000000..62dc7a2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Bottom to CD.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.082471026490066,
+        "y": 2.14360513245033
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.2422185430463575,
+        "y": 2.252566225165561
+      },
+      "isLocked": false,
+      "linkedName": "Bottom Starting Line"
+    },
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 2.75
+      },
+      "prevControl": {
+        "x": 3.8790149006622516,
+        "y": 2.485016556291391
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CD"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": "Starting",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CD to Barge.path b/src/main/deploy/pathplanner/paths/CD to Barge.path
new file mode 100644
index 0000000..5fcc008
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CD to Barge.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 2.75
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.958476027397261,
+        "y": 1.8537885273972599
+      },
+      "isLocked": false,
+      "linkedName": "CD"
+    },
+    {
+      "anchor": {
+        "x": 7.558,
+        "y": 3.9123073630136984
+      },
+      "prevControl": {
+        "x": 5.16891609589041,
+        "y": 1.8838398972602746
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Cycle AB.path b/src/main/deploy/pathplanner/paths/Cycle AB.path
new file mode 100644
index 0000000..d5b3ce6
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Cycle AB.path	
@@ -0,0 +1,90 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.9615625,
+        "y": 3.9801278409090908
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.878389547719681,
+        "y": 4.21588674319907
+      },
+      "isLocked": false,
+      "linkedName": "AB"
+    },
+    {
+      "anchor": {
+        "x": 1.7649715909090908,
+        "y": 7.220894886363636
+      },
+      "prevControl": {
+        "x": 1.8537693216543827,
+        "y": 6.987196441750741
+      },
+      "nextControl": {
+        "x": 1.608924084897191,
+        "y": 7.6315817263833186
+      },
+      "isLocked": false,
+      "linkedName": "Top Coral Station"
+    },
+    {
+      "anchor": {
+        "x": 2.9615625,
+        "y": 3.9801278409090908
+      },
+      "prevControl": {
+        "x": 2.8440210650126843,
+        "y": 4.398962959529563
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "AB"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": -45.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.9,
+      "maxWaypointRelativePos": 1.1,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Cycle CD.path b/src/main/deploy/pathplanner/paths/Cycle CD.path
new file mode 100644
index 0000000..b4fc694
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Cycle CD.path	
@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 2.75
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2075563954930053,
+        "y": 2.324270648118099
+      },
+      "isLocked": false,
+      "linkedName": "CD"
+    },
+    {
+      "anchor": {
+        "x": 1.6562086092715231,
+        "y": 0.7852235099337759
+      },
+      "prevControl": {
+        "x": 1.8315296882327805,
+        "y": 0.9634439332074864
+      },
+      "nextControl": {
+        "x": 1.4808875303102658,
+        "y": 0.6070030866600653
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 2.75
+      },
+      "prevControl": {
+        "x": 3.385559124014756,
+        "y": 2.5320786312706076
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CD"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Cycle JI.path b/src/main/deploy/pathplanner/paths/Cycle JI.path
new file mode 100644
index 0000000..e63307f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Cycle JI.path	
@@ -0,0 +1,84 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.411734271523178,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.655759646662122,
+        "y": 5.6817026555306445
+      },
+      "isLocked": false,
+      "linkedName": "Starting IJ"
+    },
+    {
+      "anchor": {
+        "x": 1.7649715909090908,
+        "y": 7.220894886363636
+      },
+      "prevControl": {
+        "x": 1.5391757596683866,
+        "y": 7.328208644075867
+      },
+      "nextControl": {
+        "x": 2.050780831649586,
+        "y": 7.085058615852309
+      },
+      "isLocked": false,
+      "linkedName": "Top Coral Station"
+    },
+    {
+      "anchor": {
+        "x": 5.411734271523178,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 4.672392038516095,
+        "y": 5.584113419996835
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Starting IJ"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.9612403100775195,
+      "maxWaypointRelativePos": 1.05,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Cycle LK.path b/src/main/deploy/pathplanner/paths/Cycle LK.path
new file mode 100644
index 0000000..edc59a7
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Cycle LK.path	
@@ -0,0 +1,84 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.357804178610237,
+        "y": 5.486653629540659
+      },
+      "isLocked": false,
+      "linkedName": "LK"
+    },
+    {
+      "anchor": {
+        "x": 1.7649715909090908,
+        "y": 7.220894886363636
+      },
+      "prevControl": {
+        "x": 1.5966241435231892,
+        "y": 7.40571679242093
+      },
+      "nextControl": {
+        "x": 1.9333190382949925,
+        "y": 7.036072980306343
+      },
+      "isLocked": false,
+      "linkedName": "Top Coral Station"
+    },
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 3.500993559994922,
+        "y": 5.391268998309367
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LK"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8997632202052086,
+      "maxWaypointRelativePos": 1.2186266771902134,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/D - E.path b/src/main/deploy/pathplanner/paths/D - E.path
new file mode 100644
index 0000000..bcce580
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/D - E.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.549886363636363,
+        "y": 2.4843892045454536
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.128238636363636,
+        "y": 2.005752840909091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.554176136363636,
+        "y": 2.4843892045454536
+      },
+      "prevControl": {
+        "x": 4.83622159090909,
+        "y": 3.0328267045454544
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/E - G.path b/src/main/deploy/pathplanner/paths/E - G.path
new file mode 100644
index 0000000..a71fded
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/E - G.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.554176136363636,
+        "y": 2.4843892045454536
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.972982954545454,
+        "y": 2.713735795454545
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.262159090909091,
+        "y": 3.86046875
+      },
+      "prevControl": {
+        "x": 6.212301136363636,
+        "y": 3.411747159090909
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path
new file mode 100644
index 0000000..64393b2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Forward and Back.path	
@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.499651445630644,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.4996514456306436,
+        "y": 7.0
+      },
+      "nextControl": {
+        "x": 5.499651445630644,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.5072811929471364,
+        "y": 6.980586730595545
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/G - J.path b/src/main/deploy/pathplanner/paths/G - J.path
new file mode 100644
index 0000000..7e802e3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/G - J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.192357954545455,
+        "y": 3.9701562499999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.192357954545455,
+        "y": 4.787826704545455
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.484375,
+        "y": 5.455923295454545
+      },
+      "prevControl": {
+        "x": 5.982954545454545,
+        "y": 5.106917613636363
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HG to Barge.path b/src/main/deploy/pathplanner/paths/HG to Barge.path
new file mode 100644
index 0000000..d388d2f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HG to Barge.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.955838815789473,
+        "y": 4.005756578947368
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.934552068302233,
+        "y": 4.621997023584086
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.5337993421052625,
+        "y": 5.035279605263158
+      },
+      "prevControl": {
+        "x": 7.212622173286315,
+        "y": 4.842237224982527
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/IJ to Barge.path b/src/main/deploy/pathplanner/paths/IJ to Barge.path
new file mode 100644
index 0000000..0a831a9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/IJ to Barge.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.19888698630137,
+        "y": 5.2195419520547945
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.23486049929036,
+        "y": 5.253929907898961
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.553042763157894,
+        "y": 5.2195419520547945
+      },
+      "prevControl": {
+        "x": 6.29554586930132,
+        "y": 5.195869163139581
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Algae",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/IJ to LK.path b/src/main/deploy/pathplanner/paths/IJ to LK.path
new file mode 100644
index 0000000..ccd38c1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/IJ to LK.path	
@@ -0,0 +1,84 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.411734271523178,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.666722238268385,
+        "y": 5.624873196871553
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.755,
+        "y": 7.171036931818182
+      },
+      "prevControl": {
+        "x": 1.954912240657937,
+        "y": 6.979269733268736
+      },
+      "nextControl": {
+        "x": 1.564159066139434,
+        "y": 7.354102416625691
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 3.490908924519007,
+        "y": 5.381530607151542
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LK"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.9344909234412,
+      "maxWaypointRelativePos": 1.3480662983425347,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/J - L.path b/src/main/deploy/pathplanner/paths/J - L.path
new file mode 100644
index 0000000..1f15b3e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/J - L.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.484375,
+        "y": 5.455923295454545
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.656732954545454,
+        "y": 5.974446022727273
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6396306818181814,
+        "y": 5.326292613636363
+      },
+      "prevControl": {
+        "x": 4.247897727272726,
+        "y": 5.904644886363636
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/L - B.path b/src/main/deploy/pathplanner/paths/L - B.path
new file mode 100644
index 0000000..88f4324
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/L - B.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6396306818181814,
+        "y": 5.326292613636363
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8119886363636355,
+        "y": 5.844815340909091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.1011363636363636,
+        "y": 4.737897727272728
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": "Rotation Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/LK to AB.path b/src/main/deploy/pathplanner/paths/LK to AB.path
new file mode 100644
index 0000000..f36c2c8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/LK to AB.path	
@@ -0,0 +1,84 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.442902780309121,
+        "y": 5.472347910370522
+      },
+      "isLocked": false,
+      "linkedName": "LK"
+    },
+    {
+      "anchor": {
+        "x": 1.7649715909090908,
+        "y": 7.220894886363636
+      },
+      "prevControl": {
+        "x": 2.030789042192635,
+        "y": 6.818654369259048
+      },
+      "nextControl": {
+        "x": 1.6271387237970898,
+        "y": 7.429466458596492
+      },
+      "isLocked": false,
+      "linkedName": "Top Coral Station"
+    },
+    {
+      "anchor": {
+        "x": 2.9615625,
+        "y": 3.9801278409090908
+      },
+      "prevControl": {
+        "x": 2.5867889091633094,
+        "y": 4.994866966222094
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.95,
+      "maxWaypointRelativePos": 1.1,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/LK to Barge.path b/src/main/deploy/pathplanner/paths/LK to Barge.path
new file mode 100644
index 0000000..e1892ea
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/LK to Barge.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.093809523257383,
+        "y": 5.630897591439201
+      },
+      "isLocked": false,
+      "linkedName": "LK"
+    },
+    {
+      "anchor": {
+        "x": 7.6,
+        "y": 5.7
+      },
+      "prevControl": {
+        "x": 4.252268835616438,
+        "y": 5.7153895547945215
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/LK to JI.path b/src/main/deploy/pathplanner/paths/LK to JI.path
new file mode 100644
index 0000000..d6ee164
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/LK to JI.path	
@@ -0,0 +1,84 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7756150493261362,
+        "y": 5.887760725794539
+      },
+      "isLocked": false,
+      "linkedName": "LK"
+    },
+    {
+      "anchor": {
+        "x": 1.6828767123287671,
+        "y": 7.353189212328767
+      },
+      "prevControl": {
+        "x": 1.5172681656002416,
+        "y": 7.54046924129733
+      },
+      "nextControl": {
+        "x": 1.9544850896817896,
+        "y": 7.046038248301759
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.411734271523178,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 4.976738681072561,
+        "y": 5.681421019377148
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Starting IJ"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.9612403100775195,
+      "maxWaypointRelativePos": 1.1369509043927652,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Middle to HG.path b/src/main/deploy/pathplanner/paths/Middle to HG.path
new file mode 100644
index 0000000..471d756
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Middle to HG.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.249544701986755,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.51127304285252,
+        "y": 3.9896688626101993
+      },
+      "isLocked": false,
+      "linkedName": "Centre Starting Line"
+    },
+    {
+      "anchor": {
+        "x": 5.803994205298014,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 6.4091035635864,
+        "y": 3.9986716795875528
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Starting",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Return Home.path b/src/main/deploy/pathplanner/paths/Return Home.path
new file mode 100644
index 0000000..fb0648c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Return Home.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.5337993421052625,
+        "y": 5.035279605263158
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.530894149921405,
+        "y": 4.273840679843942
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.5337993421052625,
+        "y": 4.015378289473684
+      },
+      "prevControl": {
+        "x": 7.514580154845477,
+        "y": 4.651815443008323
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path
new file mode 100644
index 0000000..9a0db11
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1720034246575342,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.172003424657536,
+        "y": 6.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.482791095890411,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 6.482791095890411,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path
new file mode 100644
index 0000000..b1b44d9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path
new file mode 100644
index 0000000..dad0d93
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Straight Test.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0000000000000018,
+        "y": 6.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 2.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Top to IJ.path b/src/main/deploy/pathplanner/paths/Top to IJ.path
new file mode 100644
index 0000000..e1d5e9a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Top to IJ.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.082471026490066,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.956557919698722,
+        "y": 5.192843493784709
+      },
+      "isLocked": false,
+      "linkedName": "Top Barge Starting Line"
+    },
+    {
+      "anchor": {
+        "x": 5.411734271523178,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 6.533769976910932,
+        "y": 5.220637271660809
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Starting IJ"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Starting",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Top to LK.path b/src/main/deploy/pathplanner/paths/Top to LK.path
new file mode 100644
index 0000000..b07fd81
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Top to LK.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.082471026490066,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.5109892384105965,
+        "y": 6.051676324503311
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6695454545454544,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 4.249482615894038,
+        "y": 5.6957367549668865
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LK"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": "Starting",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path
new file mode 100644
index 0000000..7d58c54
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Triangle.path
@@ -0,0 +1,91 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.138574759002216,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 4.138574759002216,
+        "y": 7.0
+      },
+      "nextControl": {
+        "x": 6.138574759002216,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.955063886077153
+      },
+      "prevControl": {
+        "x": 2.154192543186652,
+        "y": 4.703218396657915
+      },
+      "nextControl": {
+        "x": 1.8694606562423997,
+        "y": 5.1682761740235446
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 2.0255228704471047,
+        "y": 6.423228095526727
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Test",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..e221d12
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,43 @@
+{
+  "robotWidth": 0.762,
+  "robotLength": 0.762,
+  "holonomicMode": true,
+  "pathFolders": [
+    "Algae",
+    "Top Ring No rotation",
+    "Return",
+    "Rotation Test",
+    "Starting",
+    "Test"
+  ],
+  "autoFolders": [
+    "Algae Intake",
+    "Scuff",
+    "Testing"
+  ],
+  "defaultMaxVel": 3.5,
+  "defaultMaxAccel": 5.0,
+  "defaultMaxAngVel": 360.0,
+  "defaultMaxAngAccel": 360.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 52.0,
+  "robotMOI": 5.048,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.05715,
+  "driveGearing": 6.12,
+  "maxDriveSpeed": 5.4,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.19,
+  "flModuleX": 0.27305,
+  "flModuleY": 0.24765,
+  "frModuleX": 0.27305,
+  "frModuleY": -0.24765,
+  "blModuleX": -0.27305,
+  "blModuleY": 0.24765,
+  "brModuleX": -0.27305,
+  "brModuleY": -0.24765,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/deploy/rotate while driving.traj b/src/main/deploy/rotate while driving.traj
new file mode 100644
index 0000000..b2739c9
--- /dev/null
+++ b/src/main/deploy/rotate while driving.traj	
@@ -0,0 +1,78 @@
+{
+ "name":"rotate while driving",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.4909000396728516, "y":6.5899128913879395, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.6469292640686035, "y":6.570401191711426, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.4909000396728516 m", "val":2.4909000396728516}, "y":{"exp":"6.5899128913879395 m", "val":6.5899128913879395}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.6469292640686035 m", "val":6.6469292640686035}, "y":{"exp":"6.570401191711426 m", "val":6.570401191711426}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.45421],
+  "samples":[
+    {"t":0.0, "x":2.4909, "y":6.58991, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.63261, "ay":-0.09559, "alpha":16.96601, "fx":[115.31391,145.56663,111.96895,146.46487], "fy":[100.9204,-48.41461,-104.63215,45.62237]},
+    {"t":0.03382, "x":2.49526, "y":6.58986, "heading":0.0, "vx":0.25813, "vy":-0.00323, "omega":0.57377, "ax":7.70998, "ay":-0.10361, "alpha":16.43555, "fx":[117.79766,145.76818,114.30255,146.70974], "fy":[97.97316,-47.76537,-102.0438,44.78671]},
+    {"t":0.06764, "x":2.5084, "y":6.58969, "heading":0.0194, "vx":0.51887, "vy":-0.00674, "omega":1.1296, "ax":7.80044, "ay":-0.08886, "alpha":15.79543, "fx":[120.84828,146.34363,116.85868,146.68228], "fy":[94.13017,-45.92557,-99.07488,44.82414]},
+    {"t":0.10146, "x":2.53041, "y":6.58941, "heading":0.05761, "vx":0.78267, "vy":-0.00974, "omega":1.66378, "ax":7.90905, "ay":-0.06134, "alpha":14.98828, "fx":[124.74728,147.22966,119.70358,146.44253], "fy":[88.82049,-42.94184,-95.58795,45.53594]},
+    {"t":0.13528, "x":2.5614, "y":6.58905, "heading":0.11387, "vx":1.05014, "vy":-0.01182, "omega":2.17067, "ax":8.04188, "ay":-0.0352, "alpha":13.93373, "fx":[129.78535,148.33552,122.96662,146.07278], "fy":[81.17102,-38.87156,-91.3203,46.6259]},
+    {"t":0.16909, "x":2.60152, "y":6.58863, "heading":0.18728, "vx":1.32211, "vy":-0.01301, "omega":2.64189, "ax":8.20187, "ay":-0.02935, "alpha":12.55915, "fx":[136.01578,149.54526,126.77717,145.70779], "fy":[70.07483,-33.80572,-85.91359,47.6473]},
+    {"t":0.20291, "x":2.65092, "y":6.58817, "heading":0.27663, "vx":1.59949, "vy":-0.014, "omega":3.06663, "ax":8.38251, "ay":-0.06267, "alpha":10.85761, "fx":[142.82415,150.7239,131.21237,145.57568], "fy":[54.65415,-27.90188,-78.92075,47.90444]},
+    {"t":0.23673, "x":2.70981, "y":6.58766, "heading":0.38034, "vx":1.88297, "vy":-0.01612, "omega":3.43382, "ax":8.56451, "ay":-0.13937, "alpha":8.93536, "fx":[148.68033,151.73081,136.26735,146.04109], "fy":[35.44067,-21.43208,-69.74635,46.25523]},
+    {"t":0.27055, "x":2.77838, "y":6.58704, "heading":0.49647, "vx":2.17262, "vy":-0.02083, "omega":3.736, "ax":8.72812, "ay":-0.2356, "alpha":6.90316, "fx":[151.98794,152.44085,141.82989,147.59295], "fy":[15.6007,-14.8738,-57.44438,40.68734]},
+    {"t":0.30437, "x":2.85685, "y":6.5862, "heading":0.62281, "vx":2.46779, "vy":-0.0288, "omega":3.96946, "ax":8.87064, "ay":-0.32072, "alpha":4.52769, "fx":[152.75142,152.76845,147.55101,150.47702], "fy":[0.09314,-9.17268,-40.19807,27.45606]},
+    {"t":0.33819, "x":2.94538, "y":6.58504, "heading":0.75705, "vx":2.76778, "vy":-0.03965, "omega":4.12258, "ax":8.96725, "ay":-0.40599, "alpha":0.93307, "fx":[152.56893,152.65122,152.13271,152.7685], "fy":[-7.03266,-6.79716,-13.93702,0.14357]},
+    {"t":0.37201, "x":3.04411, "y":6.58347, "heading":0.89647, "vx":3.07105, "vy":-0.05338, "omega":4.15413, "ax":8.79613, "ay":-0.53779, "alpha":-5.22484, "fx":[152.66387,151.0145,149.2498,145.5505], "fy":[-3.72513,-18.35606,31.22596,-45.73568]},
+    {"t":0.40583, "x":3.153, "y":6.58135, "heading":1.03696, "vx":3.36852, "vy":-0.07156, "omega":3.97743, "ax":7.59699, "ay":-1.17566, "alpha":-15.31603, "fx":[152.3927,114.96155,121.99737,127.53892], "fy":[8.82149,-96.04456,90.77765,-83.54501]},
+    {"t":0.43964, "x":3.27126, "y":6.57826, "heading":1.17147, "vx":3.62544, "vy":-0.11132, "omega":3.45947, "ax":7.55584, "ay":-0.91387, "alpha":-16.03331, "fx":[151.30326,110.96406,118.87482,132.9486], "fy":[18.55267,-100.56755,94.08228,-74.24578]},
+    {"t":0.47346, "x":3.39819, "y":6.57397, "heading":1.28847, "vx":3.88097, "vy":-0.14223, "omega":2.91724, "ax":7.4343, "ay":-0.61232, "alpha":-17.03207, "fx":[149.42423,106.87265,112.78843,136.73623], "fy":[28.48056,-104.06757,100.22875,-66.30333]},
+    {"t":0.50728, "x":3.53369, "y":6.56881, "heading":1.38713, "vx":4.13239, "vy":-0.16294, "omega":2.34123, "ax":7.18013, "ay":-0.2645, "alpha":-18.52445, "fx":[146.41834,101.05308,101.69957,139.35679], "fy":[39.1871,-107.82495,109.84594,-59.20432]},
+    {"t":0.5411, "x":3.67755, "y":6.56315, "heading":1.4663, "vx":4.37521, "vy":-0.17188, "omega":1.71476, "ax":6.79538, "ay":0.15914, "alpha":-20.17157, "fx":[141.65844,94.74468,84.79444,141.15216], "fy":[50.78002,-109.08006,120.62607,-51.49843]},
+    {"t":0.57492, "x":3.8294, "y":6.55743, "heading":1.5243, "vx":4.60502, "vy":-0.1665, "omega":1.03258, "ax":6.23584, "ay":0.56269, "alpha":-21.31259, "fx":[134.36458,84.9136,64.74214,140.25937], "fy":[60.5362,-104.44587,125.71657,-43.52243]},
+    {"t":0.60874, "x":3.9887, "y":6.55212, "heading":1.55922, "vx":4.81591, "vy":-0.14747, "omega":0.31182, "ax":2.51954, "ay":2.54701, "alpha":-8.65482, "fx":[61.8905,21.30785,17.76223,70.46624], "fy":[61.76482,22.6527,71.16249,17.71574]},
+    {"t":0.64256, "x":4.15301, "y":6.54859, "heading":1.56976, "vx":4.90112, "vy":-0.06133, "omega":0.01912, "ax":0.021, "ay":1.03692, "alpha":-0.0408, "fx":[0.46833,0.24621,0.24643,0.46818], "fy":[17.74558,17.52983,17.74599,17.52942]},
+    {"t":0.67638, "x":4.31878, "y":6.54711, "heading":1.57041, "vx":4.90183, "vy":-0.02627, "omega":0.01774, "ax":0.00146, "ay":0.34479, "alpha":-0.00008, "fx":[0.02509,0.02468,0.02465,0.02512], "fy":[5.86505,5.86458,5.86502,5.86461]},
+    {"t":0.71019, "x":4.48455, "y":6.54642, "heading":1.57101, "vx":4.90188, "vy":-0.01461, "omega":0.01774, "ax":0.00039, "ay":0.17198, "alpha":0.00006, "fx":[0.00653,0.0068,0.00686,0.00647], "fy":[2.92509,2.92548,2.92515,2.92542]},
+    {"t":0.74401, "x":4.65033, "y":6.54602, "heading":1.57161, "vx":4.90189, "vy":-0.00879, "omega":0.01774, "ax":0.00017, "ay":0.24794, "alpha":0.0002, "fx":[0.00242,0.00351,0.00348,0.00246], "fy":[4.2169,4.21792,4.21687,4.21795]},
+    {"t":0.77783, "x":4.8161, "y":6.54587, "heading":1.57221, "vx":4.9019, "vy":-0.00041, "omega":0.01775, "ax":-0.01255, "ay":0.6938, "alpha":0.03803, "fx":[-0.31597,-0.11095,-0.1106,-0.31628], "fy":[11.69969,11.90304,11.7001,11.90264]},
+    {"t":0.81165, "x":4.98187, "y":6.54625, "heading":1.57281, "vx":4.90148, "vy":0.02306, "omega":0.01903, "ax":-2.48678, "ay":1.77879, "alpha":8.68702, "fx":[-67.78584,-18.42584,-21.31904,-61.66686], "fy":[5.39721,58.36128,6.69503,50.57347]},
+    {"t":0.84547, "x":5.14621, "y":6.54805, "heading":1.57345, "vx":4.81738, "vy":0.08321, "omega":0.31282, "ax":-6.24566, "ay":0.45912, "alpha":21.41143, "fx":[-140.22171,-66.73355,-83.49553,-134.4968], "fy":[-43.93737,123.40669,-108.13132,59.90008]},
+    {"t":0.87929, "x":5.30556, "y":6.55112, "heading":1.58403, "vx":4.60616, "vy":0.09874, "omega":1.03693, "ax":-6.60966, "ay":0.48992, "alpha":21.19107, "fx":[-143.76466,-73.9045,-94.17226,-137.87232], "fy":[-43.9427,127.28118,-110.33466,60.32951]},
+    {"t":0.91311, "x":5.45755, "y":6.55474, "heading":1.6191, "vx":4.38262, "vy":0.11531, "omega":1.75358, "ax":-7.06503, "ay":0.08522, "alpha":19.35787, "fx":[-142.92748,-94.52694,-100.18001,-143.06211], "fy":[-50.06181,115.45253,-109.47616,49.88386]},
+    {"t":0.94693, "x":5.60173, "y":6.55869, "heading":1.6784, "vx":4.14369, "vy":0.11819, "omega":2.40824, "ax":-7.38823, "ay":-0.31487, "alpha":17.65357, "fx":[-141.25761,-110.01691,-104.58004,-146.8323], "fy":[-56.07382,102.54294,-107.30868,39.41608]},
+    {"t":0.98074, "x":5.73764, "y":6.56251, "heading":1.75985, "vx":3.89383, "vy":0.10754, "omega":3.00526, "ax":-7.59703, "ay":-0.664, "alpha":16.29508, "fx":[-139.04738,-120.01517,-108.37212,-149.45837], "fy":[-62.11549,91.92777,-104.53299,29.54302]},
+    {"t":1.01456, "x":5.86498, "y":6.56577, "heading":1.86148, "vx":3.63691, "vy":0.08509, "omega":3.55634, "ax":-7.7889, "ay":-0.93433, "alpha":14.80487, "fx":[-136.70682,-127.38581,-114.59155,-151.26377], "fy":[-67.55051,82.42311,-98.31865,19.87521]},
+    {"t":1.04838, "x":5.98352, "y":6.56811, "heading":1.98175, "vx":3.3735, "vy":0.05349, "omega":4.05703, "ax":-8.89156, "ay":-0.5838, "alpha":3.17163, "fx":[-149.63334,-151.90376,-150.79951,-152.63472], "fy":[-29.85335,12.88164,-21.64003,-1.10948]},
+    {"t":1.0822, "x":6.09252, "y":6.56958, "heading":2.11896, "vx":3.0728, "vy":0.03375, "omega":4.16429, "ax":-8.93961, "ay":-0.43202, "alpha":-2.39795, "fx":[-152.3918,-150.9017,-152.83099,-152.11642], "fy":[10.55503,-24.05562,-2.78377,-13.11001]},
+    {"t":1.11602, "x":6.19133, "y":6.57048, "heading":2.25979, "vx":2.77047, "vy":0.01914, "omega":4.08319, "ax":-8.81373, "ay":-0.34481, "alpha":-5.51804, "fx":[-148.41219,-146.12098,-153.06282,-152.08011], "fy":[36.9273,-45.28775,-1.31893,-13.78127]},
+    {"t":1.14984, "x":6.27998, "y":6.57093, "heading":2.39788, "vx":2.4724, "vy":0.00748, "omega":3.89658, "ax":-8.68626, "ay":-0.27098, "alpha":-7.32834, "fx":[-144.43843,-140.80008,-153.09056,-152.67422], "fy":[50.69313,-60.055,-5.25775,-3.81772]},
+    {"t":1.18366, "x":6.35863, "y":6.57103, "heading":2.52965, "vx":2.17864, "vy":-0.00169, "omega":3.64874, "ax":-8.5645, "ay":-0.16035, "alpha":-8.72357, "fx":[-142.30968,-135.54143,-152.8386,-152.0292], "fy":[56.65843,-71.25554,-11.21067,14.89797]},
+    {"t":1.21748, "x":6.42741, "y":6.57088, "heading":2.65305, "vx":1.889, "vy":-0.00711, "omega":3.35372, "ax":-8.41732, "ay":-0.0275, "alpha":-10.25241, "fx":[-141.75868,-130.60625,-152.25491,-148.08499], "fy":[58.20424,-80.01305,-17.82377,37.76158]},
+    {"t":1.2513, "x":6.48648, "y":6.57062, "heading":2.76647, "vx":1.60434, "vy":-0.00804, "omega":3.007, "ax":-8.24298, "ay":0.07207, "alpha":-11.93451, "fx":[-142.17505,-126.17708,-151.36974,-141.12138], "fy":[57.31111,-86.87278,-24.419,58.88407]},
+    {"t":1.28511, "x":6.53602, "y":6.57039, "heading":2.86816, "vx":1.32557, "vy":-0.0056, "omega":2.60339, "ax":-8.07089, "ay":0.10693, "alpha":-13.49601, "fx":[-143.05535,-122.3737,-150.26832,-133.43666], "fy":[55.17723,-92.18101,-30.59122,74.87047]},
+    {"t":1.31893, "x":6.57624, "y":6.57026, "heading":2.9562, "vx":1.05262, "vy":-0.00199, "omega":2.14697, "ax":-7.92555, "ay":0.08723, "alpha":-14.74292, "fx":[-144.05704,-119.23541,-149.06664,-126.88639], "fy":[52.58565,-96.22565,-36.07191,85.64702]},
+    {"t":1.35275, "x":6.6073, "y":6.57024, "heading":3.02881, "vx":0.78459, "vy":0.00096, "omega":1.64838, "ax":-7.81275, "ay":0.03944, "alpha":-15.65509, "fx":[-144.97169,-116.71086,-147.88992,-121.99793], "fy":[50.07296,-99.28872,-40.68387,92.58304]},
+    {"t":1.38657, "x":6.62937, "y":6.5703, "heading":3.08456, "vx":0.52037, "vy":0.0023, "omega":1.11894, "ax":-7.72701, "ay":-0.01311, "alpha":-16.30479, "fx":[-145.68514,-114.67245,-146.85513,-118.52432], "fy":[48.01018,-101.652,-44.31692,97.06673]},
+    {"t":1.42039, "x":6.64255, "y":6.57037, "heading":3.1224, "vx":0.25905, "vy":0.00185, "omega":0.56754, "ax":-7.66005, "ay":-0.05476, "alpha":-16.78169, "fx":[-146.13969,-112.95769,-146.06082,-116.02315], "fy":[46.65159,-103.57186,-46.90369,100.09799]},
+    {"t":1.45421, "x":6.64693, "y":6.5704, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 409c8c3..3d00c4e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -35,24 +35,20 @@ public void disabledExit() {}
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    if (m_autonomousCommand != null) {
+    if (m_autonomousCommand != null)
       m_autonomousCommand.schedule();
-    }
   }
 
   @Override
   public void autonomousPeriodic() {}
 
   @Override
-  public void autonomousExit() {
-    m_robotContainer.io.chassis.adjustRotation();
-  }
+  public void autonomousExit() {}
 
   @Override
   public void teleopInit() {
-    if (m_autonomousCommand != null) {
+    if (m_autonomousCommand != null)
       m_autonomousCommand.cancel();
-    }
   }
 
   @Override
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e9550c5..df1435b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,30 +4,84 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.SignalLogger;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import frc.robot.commands.DefaultDrive;
+import frc.robot.commands.RotateChassis;
+import frc.robot.commands.ScoreReef;
+import frc.robot.swerve.Swerve;
 import frc.robot.utility.AutomatedController;
 import frc.robot.utility.IO;
-import frc.robot.utility.SwerveConstants;
+import frc.robot.utility.Util;
 
 public class RobotContainer {
+  
   public IO io = new IO();
   public final AutomatedController main;
   public final AutomatedController backup;
 
+  private final SendableChooser<Command> auto_selector;
+  Command current_auto = new PrintCommand("");
+  final SendableChooser<Integer> driver_selector = new SendableChooser<Integer>();
 
   public RobotContainer() {
     main = new AutomatedController(0, io);
     backup = new AutomatedController(1, io);
+
+    configureAuton();
+
+    auto_selector = AutoBuilder.buildAutoChooser();
+    auto_selector.onChange((command) -> {current_auto = command;});
+
+    SmartDashboard.putData("Autos", auto_selector);
+    SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule));
+
     SmartDashboard.putData("Main-Controller Mode", main.selector);
-    SmartDashboard.putData("Backup-Controller Mode", main.selector);
+    SmartDashboard.putData("Backup-Controller Mode", backup.selector);
+    SmartDashboard.putData("Driver", driver_selector);
     io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller));
-    // SmartDashboard.putData("Autonomous", ); // TBD
+
+    driver_selector.setDefaultOption("Shaan", 0);
+    driver_selector.addOption("Norah", 1);
+    driver_selector.addOption("Jason", 2);
+    driver_selector.addOption("Uriel", 3);
+    driver_selector.addOption("Debug", 4);
+    driver_selector.onChange( (driver) -> Swerve.Constants.SwitchDriver(driver));
+
+    SignalLogger.setPath("/media/sda1/ctre-logs/");
+  }
+
+
+  public void configureAuton(){
+    NamedCommands.registerCommand("Rotate to Reef", new RotateChassis(io));
+    NamedCommands.registerCommand("Face Barge", new RotateChassis(io, 0));
+    NamedCommands.registerCommand("Rotate IJ", new RotateChassis(io, -120));
+    NamedCommands.registerCommand("Rotate LK", new RotateChassis(io, 300));
+    NamedCommands.registerCommand("Rotate HG", new RotateChassis(io, 180));
+    NamedCommands.registerCommand("Rotate CD", new RotateChassis(io, 60));
+    NamedCommands.registerCommand("Rotate EF", new RotateChassis(io, 120));
+
+    NamedCommands.registerCommand("Score L-L4", new ScoreReef(io, 0, 4));
+    NamedCommands.registerCommand("Score L-L3", new ScoreReef(io, 0, 3));
+    NamedCommands.registerCommand("Score L-L2", new ScoreReef(io, 0, 2));
+    NamedCommands.registerCommand("Score L-L1", new ScoreReef(io, 0, 1));
+
+    NamedCommands.registerCommand("Score R-L4", new ScoreReef(io, 2, 4));
+    NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, 2, 3));
+    NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, 2, 2));
+    NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, 2, 1));
+
+    // NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND
+    // NamedCommands.registerCommand("Clear High Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return auto_selector.getSelected();
   }
 }
diff --git a/src/main/java/frc/robot/commands/Aimbot.java b/src/main/java/frc/robot/commands/Aimbot.java
new file mode 100644
index 0000000..7e5cbda
--- /dev/null
+++ b/src/main/java/frc/robot/commands/Aimbot.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+import frc.robot.utility.LimelightHelpers;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class Aimbot extends Command {
+
+  IO io;
+  PIDController pid = new PIDController(0.1, 0.00, 0.00);
+
+  public Aimbot(IO io) {
+    this.io = io;
+    pid.setTolerance(10);
+    addRequirements(io.limelight, io.chassis);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    double output = -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0);
+    SmartDashboard.putNumber("PID Output", output);
+    io.chassis.drive(new ChassisSpeeds(0, output, 0));
+  }
+
+  @Override
+  public void end(boolean interrupted) {}
+
+  @Override
+  public boolean isFinished() {
+    return pid.atSetpoint() || !LimelightHelpers.getTV("limelight-main");
+    }
+}
diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java
new file mode 100644
index 0000000..2a69041
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AutoAlign.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class AutoAlign extends Command {
+  IO io;
+
+  public static final int LEFT = 0;
+  public static final int CENTRE = 1;
+  public static final int RIGHT = 2;
+
+  public static final int X = 0;
+  public static final int Y = 1;
+  public static final int ROTATION = 2;
+  public static final double[][][] TagPose = {
+    // all units in meters and all rotations in degrees
+    // {x, y, rotation}     {left pole}, {center tag}, {right pole}                           **origin of blue reef is (4.48932, 4.0259), origin of red reef is (13.05831, 4.0259)
+
+    {{0.0,        0.0,       0.0}, {16.6972,  0.65532,  126}, {0.0,        0.0,       0.0}}, // TAG 1
+    {{0.0,        0.0,       0.0}, {16.6972,  7.39648,  234}, {0.0,        0.0,       0.0}}, // TAG 2
+    {{0.0,        0.0,       0.0}, {11.56081, 8.05561,  270}, {0.0,        0.0,       0.0}}, // TAG 3
+    {{0.0,        0.0,       0.0}, {9.27608,  6.137656, 0.0}, {0.0,        0.0,       0.0}}, // TAG 4
+    {{0.0,        0.0,       0.0}, {9.27608,  1.914906, 0.0}, {0.0,        0.0,       0.0}}, // TAG 5
+    {{13.6319768, 2.7594306, 0.0}, {13.47445, 3.306318, 300}, {13.9166092, 2.9237686, 0.0}}, // TAG 6
+    {{14.347698,  3.861562,  0.0}, {13.8905,  4.0259,   0.0}, {14.347698,  4.190238,  0.0}}, // TAG 7
+    {{13.9166092, 5.128133,  0.0}, {13.47445, 4.745482, 60 }, {13.6319768, 5.292471,  0.0}}, // TAG 8
+    {{12.4853192, 5.292471,  0.0}, {12.64336, 4.745482, 120}, {12.2006868, 5.128133,  0.0}}, // TAG 9
+    {{11.769598,  4.190238,  0.0}, {12.22731, 4.0259,   180}, {11.769598,  3.861562,  0.0}}, // TAG 10
+    {{12.2002296, 2.9247686, 0.0}, {12.64336, 3.306318, 240}, {12.4853192, 2.7594306, 0.0}}, // TAG 11
+    {{0.0,        0.0,       0.0}, {0.851154, 0.65532,  54 }, {0.0,        0.0,       0.0}}, // TAG 12
+    {{0.0,        0.0,       0.0}, {0.851154, 7.39648,  306}, {0.0,        0.0,       0.0}}, // TAG 13
+    {{0.0,        0.0,       0.0}, {8.272272, 6.137656, 180}, {0.0,        0.0,       0.0}}, // TAG 14
+    {{0.0,        0.0,       0.0}, {8.272272, 1.914906, 180}, {0.0,        0.0,       0.0}}, // TAG 15 
+    {{0.0,        0.0,       0.0}, {5.987542, -0.00381, 90 }, {0.0,        0.0,       0.0}}, // TAG 16
+    {{3.6320984,  2.9237686, 0.0}, {4.073906, 3.306318, 240}, {3.9160704,  2.7594306, 0.0}}, // TAG 17
+    {{3.2004,     4.190238,  0.0}, {3.6576,   4.0259,   180}, {3.2004,     3.861562,  0.0}}, // TAG 18
+    {{3.9160704,  5.292471,  0.0}, {4.073906, 4.745482, 120}, {3.6320984,  5.128133,  0.0}}, // TAG 19 
+    {{5.3471572,  5.128133,  0.0}, {4.90474,  4.745482, 60 }, {5.0625248,  5.292471,  0.0}}, // TAG 20
+    {{5.777738,   3.861562,  0.0}, {5.321046, 4.0259,   0.0}, {5.777738,   4.190238,  0.0}}, // TAG 21
+    {{5.0625248,  2.7594306, 0.0}, {4.90474,  3.306318, 300}, {5.3465476,  2.9237686, 0.0}}  // TAG 22
+  };
+
+  // will eventually be seperatly adjusted
+  PIDController pidY = new PIDController(0.10, 0.00, 0.00);
+  PIDController pidX = new PIDController(0.10, 0.00, 0.00);
+  PIDController pidR = new PIDController(0.01, 0.00, 0.00);
+
+  public double poleX;
+  public double poleY;
+  public double tagRotation;
+  int side;
+  int tag;
+  int zone;
+
+  public AutoAlign(int side /** make sure that the number is either 0 or 2*/, IO io) {
+    this.side = side;
+    this.io = io;
+    pidR.enableContinuousInput(0.0, 360.0); // CHECK IF THIS ACTUALLY ALLOWS WRAPPING
+  }
+
+  @Override
+  public void initialize() {
+    // find zone first
+    Boolean cZone = DriverStation.getAlliance().get() == Alliance.Blue;
+    zone = (cZone)? 
+    (int) Math.atan((io.chassis.pose().getY()- 4.0259)/(io.chassis.pose().getX()-4.48932)): 
+    (int) Math.atan((io.chassis.pose().getY()- 4.0259)/(io.chassis.pose().getX()-13.05831));
+    zone = zone/60;
+
+    switch (zone) {
+      case 1:
+        tag = (cZone)? 20: 8;
+        break;
+      case 2:
+        tag = (cZone)? 19: 9;
+        break;
+      case 3:
+        tag = (cZone)? 18: 10;
+        break;
+      case 4:
+        tag = (cZone)? 17: 11;
+        break;
+      case 5:
+        tag = (cZone)? 22: 6;
+        break;
+      default:
+        tag = (cZone)? 21: 7;
+        break;
+    }
+    // then check if the zone 
+
+    // tag = zone
+    // poleX = TagPose[tag][side][X];
+    // poleY = TagPose[tag][side][Y];
+    poleX = TagPose[tag - 1][side][X];
+    poleY = TagPose[tag - 1][side][Y];
+    tagRotation = TagPose[tag - 1][CENTRE][ROTATION];
+  }
+
+  @Override
+  public void execute() {
+    double currentRotation = io.chassis.getYaw();
+    double currentPositionX = io.chassis.pose().getX();
+    double currentPositionY = io.chassis.pose().getY();
+
+
+    io.chassis.drive(new ChassisSpeeds(pidY.calculate(currentPositionY, poleY), pidX.calculate(currentPositionX, poleX), pidR.calculate(currentRotation, tagRotation)));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    io.chassis.drive(new ChassisSpeeds());
+  }
+
+  @Override
+  public boolean isFinished() {
+    return (pidX.atSetpoint() && pidY.atSetpoint() && pidR.atSetpoint());
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ClearAlgae.java b/src/main/java/frc/robot/commands/ClearAlgae.java
new file mode 100644
index 0000000..e896c9c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ClearAlgae.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import frc.robot.utility.IO;
+import frc.robot.utility.Util;
+
+public class ClearAlgae extends SequentialCommandGroup {
+
+  public ClearAlgae(IO io) {
+    addCommands(
+      io.elevator.moveCommand(0),
+      new WaitUntilCommand(.2), // Wait to be at the bottom
+      Util.Do(() -> {
+        io.shooter.angle(0);
+        io.shooter.speed(-1);
+      }, io.shooter), // TODO: See if we need a seperate angle for algae clearing
+      io.elevator.moveCommand(4), // TODO: See if we can get away with L3 height 
+      new WaitUntilCommand(.5), // Wait to be at the Top
+      Util.Do(() -> {
+        io.shooter.speed(0);
+      }, io.shooter),
+      io.elevator.moveCommand(0) // TODO: See if we can get away with L3 height 
+    );
+  }
+}
diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java
index b631bc0..f046aa7 100644
--- a/src/main/java/frc/robot/commands/DefaultDrive.java
+++ b/src/main/java/frc/robot/commands/DefaultDrive.java
@@ -3,9 +3,9 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.utility.Util;
+import frc.robot.swerve.Swerve;
+import frc.robot.swerve.Swerve.Constants;
 import frc.robot.utility.IO;
-import frc.robot.utility.SwerveConstants;
 
 import java.util.function.DoubleSupplier;
 
@@ -22,9 +22,9 @@ public DefaultDrive(IO io, ChassisSpeeds chassisSpeeds) {
     }
 
     public DefaultDrive(IO io, CommandXboxController controller) {
-        this(io, () -> modifyAxis(controller.getLeftY()) * SwerveConstants.MAX_VELOCITY,
-        () -> modifyAxis(controller.getLeftX()) * SwerveConstants.MAX_VELOCITY,
-        () -> modifyAxis(controller.getRightX()) * SwerveConstants.MAX_VELOCITY);
+        this(io, () -> -modifyAxis(controller.getLeftY()) * Swerve.Constants.MAX_VELOCITY,
+        () -> -modifyAxis(controller.getLeftX()) * Swerve.Constants.MAX_VELOCITY,
+        () -> -modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_ANGULAR_VELOCITY);
         this.controller = controller;
     }
   
@@ -44,18 +44,17 @@ public DefaultDrive(IO io,
     @Override
     public void execute() {
         double down_scale = 1.25 - modifyAxis(controller.getLeftTriggerAxis());
-        double up_scale = (SwerveConstants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis());
+        double up_scale = (Swerve.Constants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis());
 
-        double scale = 0.8 * down_scale + up_scale;
-        double rot_scale = .48 * down_scale + up_scale; //0.48 for Shaan. 0.6 for Tristan.
+        double scale = Constants.transFactor * down_scale + up_scale;
+        double rot_scale = Constants.rotFactor * down_scale;
 
         double xSpeed = x_supplier.getAsDouble() * scale;
         double ySpeed = y_supplier.getAsDouble() * scale;
         double rotationSpeed = rotation_supplier.getAsDouble() * down_scale * rot_scale;
 
-        
         ChassisSpeeds output = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
-
+    
         if (io.chassis.field_oritented)
             output = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed, io.chassis.rotation());
 
diff --git a/src/main/java/frc/robot/commands/GrabAlgae.java b/src/main/java/frc/robot/commands/GrabAlgae.java
deleted file mode 100644
index 6a9cf60..0000000
--- a/src/main/java/frc/robot/commands/GrabAlgae.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.utility.IO;
-
-public class GrabAlgae extends Command {
-  IO io;
-
-  public GrabAlgae(IO io) {
-    this.io = io;
-    addRequirements(io.intake);
-  }
-
-  @Override
-  public void initialize() {}
-
-  @Override
-  public void execute() {
-    io.intake.volts(12);
-  }
-
-  @Override
-  public void end(boolean interrupted) {
-    io.intake.stop();
-  }
-
-  @Override
-  public boolean isFinished() {
-    return io.intake.grabbed();
-  }
-}
diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java
new file mode 100644
index 0000000..bf46867
--- /dev/null
+++ b/src/main/java/frc/robot/commands/Intake.java
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class Intake extends Command {
+
+  Runnable intake;
+  Runnable stop;
+  BooleanSupplier holding;
+  double angle;
+  boolean release;
+
+  // public static int INTAKE_CORAL = -1;
+  // public static int INTAKE_ALGAE = -2;
+  // public static int SCORE_CORAL = 1;
+  // public static int SCORE_ALGAE = 2;
+  // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP
+
+  public Intake(IO io, boolean release, GenericHID controller) {
+    holding = () -> (release) ? io.shooter.coral() :  !io.shooter.coral();
+    
+    intake = () -> {
+      io.shooter.speed((release) ? 1 : .4);
+    };
+
+    stop = () -> {
+      io.shooter.stopIntake();
+      controller.setRumble(RumbleType.kBothRumble, 0.0);
+    };
+
+    this.release = release;
+  }
+
+  public Intake(IO io, boolean release){
+    this(io, release, null);
+  }
+
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    intake.run();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    stop.run();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return holding.getAsBoolean();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/LimelightAlign.java b/src/main/java/frc/robot/commands/LimelightAlign.java
new file mode 100644
index 0000000..dd21e1c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LimelightAlign.java
@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+import frc.robot.utility.LimelightHelpers;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LimelightAlign extends Command {
+
+  IO io;
+  int positions;
+  PIDController pid = new PIDController(.1, 0, 0);
+  String limelight = "limelight-main";
+  Runnable exec;
+
+  public LimelightAlign(IO io, int positions, Boolean rotation) {
+    this.io = io;
+    this.positions = positions;
+
+    exec = (rotation) ? () -> {io.chassis.drive(new ChassisSpeeds(0, 0, -pid.calculate(LimelightHelpers.getTX("limelight-main"))));} : () -> io.chassis.drive(new ChassisSpeeds(0, -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0));
+
+    pid.setTolerance(1);
+    addRequirements(io.chassis);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    switch (positions) {
+      case 2: // Right
+    LimelightHelpers.SetFidcuial3DOffset(limelight, 0,0.2, 0);
+        break;
+      case 1: // Center
+      LimelightHelpers.SetFidcuial3DOffset(limelight, 0, 0, 0);
+        break;
+      default:// Left
+      LimelightHelpers.SetFidcuial3DOffset(limelight, 0, -0.2, 0);
+        break;
+    }
+    LimelightHelpers.Flush();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    io.chassis.drive(new ChassisSpeeds(0, -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    io.chassis.drive(new ChassisSpeeds());
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    // return pid.atSetpoint() || LimelightHelpers.getTV("limelight-main");
+    return pid.atSetpoint();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java
deleted file mode 100644
index a532b8b..0000000
--- a/src/main/java/frc/robot/commands/ReleaseAlgae.java
+++ /dev/null
@@ -1,35 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.utility.IO;
-
-public class ReleaseAlgae extends Command {
-  IO io;
-
-  double speed;
-
-  public ReleaseAlgae(IO io, boolean spit_out) {
-    this.io = io;
-
-    speed = (spit_out) ? -12 : 12;
-    addRequirements(io.intake);
-  }
-
-  @Override
-  public void initialize() {}
-
-  @Override
-  public void execute() {
-    io.intake.volts(speed);
-  }
-
-  @Override
-  public void end(boolean interrupted) {
-    io.intake.stop();
-  }
-
-  @Override
-  public boolean isFinished() {
-    return !io.intake.grabbed();
-  }
-}
diff --git a/src/main/java/frc/robot/commands/RotateChassis.java b/src/main/java/frc/robot/commands/RotateChassis.java
new file mode 100644
index 0000000..0053e45
--- /dev/null
+++ b/src/main/java/frc/robot/commands/RotateChassis.java
@@ -0,0 +1,77 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+
+public class RotateChassis extends Command {
+
+  IO io;
+  double targetAngle;
+  boolean automated = false;
+  Translation2d reef = null;
+  final PIDController rotationPID = new PIDController(0.01, 0.0, 0.001);
+
+  public RotateChassis(IO io, double targetAngle) {
+    this.io = io;
+    this.targetAngle = targetAngle;
+    rotationPID.setTolerance(5);
+    rotationPID.enableContinuousInput(0, 360);
+    automated = false;
+    addRequirements(io.chassis);
+  }
+
+  public RotateChassis(IO io){
+    this(io, 0);  
+    boolean blue = DriverStation.getAlliance().get() == Alliance.Blue;
+    reef = (blue) ? new Translation2d(4.5, 
+    4) : new Translation2d(13.25, 4) ;
+    automated = true;
+  }
+
+  @Override
+  public void initialize() {
+    if (automated){
+      Translation2d mPose = io.chassis.pose().getTranslation();
+      double reefBotAngle = (Math.toDegrees(Math.atan2(mPose.getX()-reef.getX(), mPose.getY()-reef.getY())) + 540.0) % 360;
+
+      SmartDashboard.putNumber("Reef to Bot Angle", reefBotAngle);
+      
+      switch ( ((int)reefBotAngle) / 60) {
+        case 1: targetAngle = 240; //  -120
+          break;
+        case 2: targetAngle = 300; // -60
+          break;
+        case 3: targetAngle = 0; // 0
+          break;
+        case 4: targetAngle = 60; // 60
+          break;
+        case 5: targetAngle = 120; // 120
+          break;
+        default: targetAngle = 180; // 180
+          break;
+      }
+    }
+  }
+
+  @Override
+  public void execute() {
+    io.chassis.drive(new ChassisSpeeds(0, 0, rotationPID.calculate(io.chassis.getYaw(), targetAngle)));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    io.chassis.drive(new ChassisSpeeds());
+  }
+
+  @Override
+  public boolean isFinished() {
+    // return Math.abs(rotationPID.getError()) < 1;
+    return rotationPID.atSetpoint();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java
new file mode 100644
index 0000000..2e8209e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ScoreReef.java
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import frc.robot.utility.IO;
+import frc.robot.utility.Util;
+
+public class ScoreReef extends SequentialCommandGroup {
+
+  public ScoreReef(IO io, int reefPosition, int level) {
+    addCommands(
+      new LimelightAlign(io, reefPosition, false),
+      io.elevator.moveCommand(level),
+      new WaitUntilCommand(io.elevator::atPosition),
+      Util.Do(() -> io.shooter.angle(level), io.shooter),
+      new Intake(io, true), // TODO: Set to Reef Scoring Angle
+      io.elevator.moveCommand(0)
+    );
+  }
+}
diff --git a/src/main/java/frc/robot/commands/SimpleAlign.java b/src/main/java/frc/robot/commands/SimpleAlign.java
new file mode 100644
index 0000000..7e7f1f9
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SimpleAlign.java
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.utility.IO;
+import frc.robot.utility.Util;
+
+public class SimpleAlign extends Command {
+  IO io;
+  boolean right;
+  Timer time = new Timer();
+
+  public SimpleAlign(IO io, boolean right) {
+    this.io = io;
+    this.right = right;
+    addRequirements(io.chassis);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    time.restart();
+    io.chassis.drive(new ChassisSpeeds(0, ((right) ? 1 : -1) * (double) Util.get("Align Speed", 2.5), 0));
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    io.chassis.drive(new ChassisSpeeds());
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return time.hasElapsed((double) Util.get("Align Time", .2));
+  }
+}
diff --git a/src/main/java/frc/robot/modules/KrakenSwerveModule.java b/src/main/java/frc/robot/modules/KrakenSwerveModule.java
deleted file mode 100644
index 12c5f90..0000000
--- a/src/main/java/frc/robot/modules/KrakenSwerveModule.java
+++ /dev/null
@@ -1,126 +0,0 @@
-package frc.robot.modules;
-
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.SparkMax;
-
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.reduxrobotics.sensors.canandmag.Canandmag;
-import com.reduxrobotics.sensors.canandmag.CanandmagSettings;
-
-public class KrakenSwerveModule {
-    public final TalonFX driveMotor;
-    public final SparkMax steerMotor;
-    public final Canandmag steerEncoder;
-
-    double desiredAngle;
-
-    final double PI2 = 2.0 * Math.PI;
-
-    public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
-    public static final double DRIVE_REDUCTION = (15.0 / 32.0) * (10.0 / 60.0);
-    public static final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0);
-    public static final double DRIVE_CONVERSION_FACTOR = Math.PI * WHEEL_DIAMETER * DRIVE_REDUCTION;
-
-    public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerCANID) {
-        driveMotor = new TalonFX(driveID, "rio");
-        steerMotor = new SparkMax(steerID, MotorType.kBrushless);
-        steerEncoder = new Canandmag(steerCANID);
-
-        SparkMaxConfig steerConfig = new SparkMaxConfig();
-
-        steerConfig
-                .smartCurrentLimit(20)
-                .idleMode(IdleMode.kCoast)
-                .inverted(false);
-
-        steerConfig.encoder
-                .positionConversionFactor(2 * Math.PI * STEER_REDUCTION)
-                .velocityConversionFactor(Math.PI * STEER_REDUCTION / 60);
-
-        steerConfig.closedLoop
-                .positionWrappingEnabled(true)
-                .positionWrappingMaxInput(PI2)
-                // .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-                .pid(0.2, 0.0, 0.0);
-
-        steerConfig.signals.primaryEncoderPositionAlwaysOn(false);
-        steerConfig.signals.primaryEncoderPositionPeriodMs(20);
-
-        CanandmagSettings settings = new CanandmagSettings();
-        settings.setInvertDirection(true);
-
-        steerEncoder.clearStickyFaults();
-        steerEncoder.setSettings(settings);
-
-        TalonFXConfiguration config = new TalonFXConfiguration();
-
-        config.CurrentLimits.StatorCurrentLimit = 80;
-        config.CurrentLimits.StatorCurrentLimitEnable = true;
-
-        config.CurrentLimits.SupplyCurrentLimit = 20;
-        config.CurrentLimits.SupplyCurrentLimitEnable = true;
-        
-        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-
-        config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-        steerMotor.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        steerMotor.getEncoder().setPosition(angle());
-
-        driveMotor.getConfigurator().apply(config);
-
-        tab.addDouble("Absolute Angle", () -> Math.toDegrees(angle()));
-        // tab.addDouble("Current Angle", () -> Math.toDegrees(steerMotor.getEncoder().getPosition()));
-        tab.addDouble("Angle Difference", () -> Math.toDegrees(angle() - steerMotor.getEncoder().getPosition()));
-        tab.addDouble("Target Angle", () -> Math.toDegrees(desiredAngle));
-        tab.addBoolean("Active", steerEncoder::isConnected);
-    }
-
-    public void resetDrivePosition() {
-        driveMotor.setPosition(0.0);
-    }
-
-    public void syncSteerEncoders() {
-        steerMotor.getEncoder().setPosition(angle());
-    }
-
-    public void resetAbsolute() {
-        steerEncoder.setAbsPosition(0, 250);
-    }
-
-    public double drivePosition() {
-        return driveMotor.getPosition().getValueAsDouble() * .502 * WHEEL_DIAMETER;
-    }
-
-    public double velocity() {
-        return driveMotor.getRotorVelocity().getValueAsDouble() * PI2 * .502 * Units.inchesToMeters(3);
-    }
-
-    public double angle() {
-        return (steerEncoder.getAbsPosition() * PI2) % PI2;
-    }
-
-    public void stop() {
-        driveMotor.stopMotor();
-        steerMotor.stopMotor();
-    }
-
-    public void set(double driveVolts, double targetAngle) {
-        syncSteerEncoders();
-
-        driveMotor.set(driveVolts);
-        steerMotor.getClosedLoopController().setReference(targetAngle, ControlType.kPosition);
-    }
-
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java
deleted file mode 100644
index f573422..0000000
--- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java
+++ /dev/null
@@ -1,44 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class AlgaeIntake extends SubsystemBase {
-  public SparkMax roller = new SparkMax(9, MotorType.kBrushless);
-  public DigitalInput beamBreak = new DigitalInput(0);
-  SparkMaxConfig rollerConfig = new SparkMaxConfig();
-
-  public AlgaeIntake() {
-    rollerConfig.idleMode(SparkMaxConfig.IdleMode.kBrake);
-
-    roller.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-  }
-
-  public void volts(double volts){
-    roller.setVoltage(volts);
-  }
-
-  public void speed(double speed){
-    roller.setVoltage(speed);
-  }
-
-  public void stop(){
-    roller.stopMotor();
-  }
-
-  public boolean grabbed(){
-    return beamBreak.get();
-  }
-
-  @Override
-  public void periodic() {
-    SmartDashboard.putBoolean("Intake Full", grabbed());
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index 984e633..91aa9a5 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -1,114 +1,202 @@
 package frc.robot.subsystems;
 
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.*;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
+import frc.robot.utility.Util;
 
 public class Elevator extends SubsystemBase {
-  public SparkMax motor = new SparkMax(7, MotorType.kBrushless);;
-  public SparkMax follower = new SparkMax(8, MotorType.kBrushless);
-  SparkMaxConfig config = new SparkMaxConfig();
+  TalonFX lead = new TalonFX(11);
+  TalonFX follow = new TalonFX(12);
+
+  LiDARDio lidar = new LiDARDio(4);
+
+  boolean selectAbsolute = true; // NOTE: CAN SET TO FALSE IN CASE OF EMERGENCIES LIKE CORAL BLOCKING LIDAR
 
   TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500));
+  public boolean stopped = true;
   Timer time = new Timer();
   double target = 0;
-  boolean stopped = true;
 
-  // Target Heights
+  PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0);
+
+  final String[] levelLayout = { "Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae" };
+
+  public final double[] Level = { 0, 25, 43.5, 76, 110 };
   public final double Rest = 0;
-  public final double L2 = 0;
-  public final double L3 = 0;
-  public final double L4 = 0;
-  public final double Barge = 0;
-  public final double MAX_HEIGHT = 0;
+  public static final double L1 = (double) Util.get("L1 Height", 25.0);
+  public static final double L2 = (double) Util.get("L2 Height", 43.5);
+  public static final double L3 = (double) Util.get("L3 Height", 76.0);
+  public static final double L4 = (double) Util.get("L4 Height", 110.0);
+  public TalonFXConfiguration config = new TalonFXConfiguration();
 
+  boolean redundancy = false;
 
-  public Elevator() {
-    config
-      .idleMode(SparkMaxConfig.IdleMode.kBrake)
-      .closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-      .pid(0, 0, 0);
-    
-    motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+  public boolean softLimits = false;
 
-    config.follow(motor, true);
+  public Elevator() {
+    config.Slot0.kP = 0.3;
+    config.Slot0.kI = 0.0;
+    config.Slot0.kD = 0.1;
+    config.Slot0.kG = 0;
+
+    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.SoftwareLimitSwitch
+        .withForwardSoftLimitEnable(softLimits)
+        .withForwardSoftLimitThreshold(160.0)
+        .withReverseSoftLimitEnable(softLimits)
+        .withReverseSoftLimitThreshold(0.0);
+
+    lead.getConfigurator().apply(config);
+    follow.setControl(new Follower(lead.getDeviceID(), true));
+  }
 
-    follower.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+  public void PID(double p, double i, double d, double g) {
+    config.Slot0.kP = p;
+    config.Slot0.kI = i;
+    config.Slot0.kD = d;
+    config.Slot0.kG = g;
+    lead.getConfigurator().apply(config);
+  }
 
-    zero();
+  public void toggleSoftLimits() {
+    softLimits = !softLimits;
+    lead.getConfigurator().apply(
+        new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(softLimits).withReverseSoftLimitEnable(softLimits));
   }
 
   public void speed(double speed) {
-    motor.set(speed);
+    lead.set(speed);
   }
-  
+
   public void volts(double volts) {
-    motor.setVoltage(volts);
+    lead.setVoltage(volts);
   }
 
-  public void stop(){
+  public void stop() {
     stopped = true;
-    motor.stopMotor();
-  }
-
-  public void movePID(double height) {
-    motor.getClosedLoopController().setReference(height, ControlType.kPosition);
+    lead.stopMotor();
+    follow.stopMotor();
   }
 
-  public void move(double height){ // TODO: Checkout how adding a feedforward affects the results
+  public void move(double height) {
     stopped = false;
-    target = Math.max( Math.min( height, 0 ), MAX_HEIGHT );
+    target = height;
     time.restart();
   }
 
-  public double elevatorPos() {
-    return motor.getEncoder().getPosition();
+  public void move(int level) {
+    move(Level[Math.min(0, Math.max(4, level))]);
+    SmartDashboard.putString("Target Elevator Level", levelLayout[level]);
+  }
+
+  public InstantCommand moveCommand(int level) {
+    return new InstantCommand(() -> this.move(level), this);
   }
 
-  public void zero(){
-    motor.getEncoder().setPosition(0);
+  public void zero() {
+    lead.setPosition(0);
   }
 
-  public void rest(){
-    move(Rest);
+  public boolean atPosition() {
+    return profile.isFinished(time.get());
   }
 
-  public void L2(){
-    move(L2);
+  public Voltage voltage() {
+    return lead.getMotorVoltage().getValue();
   }
 
-  public void L3(){
-    move(L3);
+  public double position() {
+    // return lead.getPosition().getValueAsDouble() * conversion;
+    return lead.getPosition().getValueAsDouble(); 
   }
 
-  public void L4(){
-    move(L4);
+  public boolean absoluteConnected(){
+    return lidar.connected();
   }
 
+  public LinearVelocity velocity() {
+    // return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() *
+    // conversion);
+    return MetersPerSecond.of(lead.getVelocity().getValueAsDouble());
+  }
+
+  public void toggleRedundancy(){
+    redundancy = !redundancy;
+  }
+
+  final double gearReduction = 1 / 17;
+  final double conversion = Math.PI * gearReduction * (Units.inchesToMeters(2) / 60);
+
+  public final SysIdRoutine routine = new SysIdRoutine(new Config(
+      null,
+      Volts.of(4),
+      Seconds.of(5),
+      (state) -> SignalLogger.writeString("state", state.toString())),
+      new Mechanism(
+          volts -> lead.setControl(new VoltageOut(volts.in(Volts))),
+          log -> {
+            log.motor("Elevator")
+                .voltage(voltage())
+                .linearPosition(Meters.of(position()))
+                .linearVelocity(velocity());
+          }, this));
+
   @Override
   public void periodic() {
-    if (stopped) return;
-
-    State out = profile.calculate(time.get(), new State(L2, Barge), new State(target, 0));
-    motor.getClosedLoopController().setReference(out.position, ControlType.kPosition);
-    
-    // TODO: Maybe log Supplied Volts
-    SmartDashboard.putNumber("Elevator Height", motor.getEncoder().getPosition());
-    
+    SmartDashboard.putNumber("Elevator Height", position());
+    SmartDashboard.putNumber("Elevator Height LiDAR", lidar.distance());
+    SmartDashboard.putBoolean("Elevator LiDAR Connected", absoluteConnected());
+    SmartDashboard.putBoolean("Elevator Soft Limits Active", softLimits);
+
+    double cTime = time.get();
+
+    if (stopped)
+      return;
+
+    if(absoluteConnected() && selectAbsolute) {
+      lead.setPosition(lidar.distance());
+    }
+
+    State out = profile.calculate(cTime, new State(L2, lead.getVelocity().getValueAsDouble()), new State(target, 0));
+    lead.setControl(positionRequest.withPosition(out.position).withEnableFOC(true));
+
+    stopped = profile.isFinished(cTime);
+
+    SmartDashboard.putNumber("Elevator Motor Voltage", voltage().magnitude());
+
     SmartDashboard.putNumber("Elevator Target Height", target);
     SmartDashboard.putNumber("Elevator cTarget Height", out.position);
 
-    SmartDashboard.putNumber("Elevator Speed", motor.getEncoder().getVelocity());
+    SmartDashboard.putNumber("Elevator Velocity", velocity().magnitude());
     SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity);
+
+    SmartDashboard.putBoolean("Elevator Soft Limits", softLimits);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java
index 728aae5..e208296 100644
--- a/src/main/java/frc/robot/subsystems/Hang.java
+++ b/src/main/java/frc/robot/subsystems/Hang.java
@@ -1,34 +1,65 @@
 package frc.robot.subsystems;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.utility.Util;
 
 public class Hang extends SubsystemBase {
-  public TalonFX hang = new TalonFX(0, "rio");
+  // public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet
+  SparkMax motor = new SparkMax(16, MotorType.kBrushed); // CIM
+  public SparkMaxConfig config = new SparkMaxConfig();
 
-  public static final double HANG_MAX_ANGLE = 0;
+  public boolean up = false;
+  public static final double upPosition = (double) Util.get("Hang Up Position", 0);
+  public static final double downPosition = (double) Util.get("Hang Down Position", 0);
+  
+  public static final double kP = (double) Util.get("Hang kP", 0.0);
+  public static final double kI = (double) Util.get("Hang kI", 0.0);
+  public static final double kD = (double) Util.get("Hang kD", 0.0);
 
   public Hang() {
-    hang.setNeutralMode(NeutralModeValue.Brake);
+    config.idleMode(SparkMaxConfig.IdleMode.kBrake);
+    config.closedLoop.pid(kP, kI, kD, ClosedLoopSlot.kSlot0);
+
+    motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+  }
+
+  public void PID(double k, double i, double p){
+    config.closedLoop.pid(k, i, p);
+    motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+  }
+
+  public void speed(double speed) {
+    motor.set(speed);
+  }
+
+  public void volts(double volts) {
+    motor.setVoltage(volts);
   }
 
-  public void hangSpeed(double speed) {
-    hang.set(speed);
+  public void stop() {
+    motor.stopMotor();
   }
 
-  public void hangVoltage(double volts) {
-    hang.setVoltage(volts);
+  public void toggleHang(){
+    up = !up;
+    motor.getClosedLoopController().setReference(((up) ? upPosition : downPosition), ControlType.kPosition);
   }
 
-  public void stopHang() {
-    hang.stopMotor();
+  public double angle() {
+    return motor.getEncoder().getPosition();
   }
 
   @Override
   public void periodic() {
-    SmartDashboard.putNumber("Hang Pos", hang.getPosition().getValueAsDouble());
+    SmartDashboard.putNumber("Hang Position", motor.getEncoder().getPosition());
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/LiDAR.java b/src/main/java/frc/robot/subsystems/LiDAR.java
new file mode 100644
index 0000000..5eb24bf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LiDAR.java
@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LiDAR extends SubsystemBase {
+  public enum LidarConfiguration {
+    DEFAULT, // Default mode, balanced performance
+    SHORT_RANGE, // Short range, high speed
+    DEFAULT_HIGH_SPEED, // Default range, higher speed short range
+    MAXIMUM_RANGE, // Maximum range
+    HIGH_SENSITIVE, // High sensitivity detection, high erroneous measurements
+    LOW_SENSITIVE // Low sensitivity detection, low erroneous measurements
+  }
+
+  I2C _lidar;
+  int config = 0;
+  String[] display = {"Default", "Short Range", "Default High Speed", "Maximum Range", "High Sensetivity", "Low Sensetivity"};
+  byte[] distanceBuffer = new byte[2];
+
+  public LiDAR() {
+    this(LidarConfiguration.DEFAULT, (byte) 0x62);
+  }
+
+  public LiDAR(LidarConfiguration configuration, int address) {
+    _lidar = new I2C(Port.kMXP, 0x62); // default i2c port
+    // if the address isn't 0x62 (default address) change the address
+    if (address != 0x62) {
+      setI2cAddress((byte) address);
+    }
+
+    changeMode(configuration);
+  }
+
+  private void setI2cAddress(byte newAddress) {
+    // read and write back serial number
+    // to ensure that we have the correct sensor
+    byte[] dataBytes = new byte[2];
+    _lidar.read(0x8F, 2, dataBytes);
+    _lidar.write(0x18, dataBytes[0]);
+    _lidar.write(0x19, dataBytes[1]);
+
+    // Write the new I2C device address to registers
+    dataBytes[0] = newAddress;
+    _lidar.write(0x1a, dataBytes[0]);
+
+    // Enable the new I2C device address using the default I2C device address
+    dataBytes[0] = 0;
+    _lidar.write(0x1e, dataBytes[0]);
+
+    // delete old object, create new one at new address
+    _lidar = null;
+    _lidar = new I2C(Port.kMXP, newAddress);
+
+    // disable default I2C device address (using the new I2C device address)
+    dataBytes[0] = (1 << 3); // set bit to disable default address
+    _lidar.write(0x1e, dataBytes[0]);
+  }
+
+  public void changeMode(LidarConfiguration configuration) {
+    switch (configuration) {
+      case DEFAULT: // Default mode, balanced performance
+        _lidar.write(0x02, 0x80); // Default
+        _lidar.write(0x04, 0x08); // Default
+        _lidar.write(0x1c, 0x00); // Default
+        break;
+
+      case SHORT_RANGE: // Short range, high speed
+        _lidar.write(0x02, 0x1d);
+        _lidar.write(0x04, 0x08); // Default
+        _lidar.write(0x1c, 0x00); // Default
+        break;
+
+      case DEFAULT_HIGH_SPEED: // Default range, higher speed short range
+        _lidar.write(0x02, 0x80); // Default
+        _lidar.write(0x04, 0x00);
+        _lidar.write(0x1c, 0x00); // Default
+        break;
+
+      case MAXIMUM_RANGE: // Maximum range
+        _lidar.write(0x02, 0xff);
+        _lidar.write(0x04, 0x08); // Default
+        _lidar.write(0x1c, 0x00); // Default
+        break;
+
+      case HIGH_SENSITIVE: // High sensitivity detection, high erroneous measurements
+        _lidar.write(0x02, 0x80); // Default
+        _lidar.write(0x04, 0x08); // Default
+        _lidar.write(0x1c, 0x80);
+        break;
+
+      case LOW_SENSITIVE: // Low sensitivity detection, low erroneous measurements
+        _lidar.write(0x02, 0x80); // Default
+        _lidar.write(0x04, 0x08); // Default
+        _lidar.write(0x1c, 0xb0);
+        break;
+    }
+    config = configuration.ordinal();
+  }
+
+  public double distance(boolean biasCorrection) {
+    _lidar.write(0x00, (biasCorrection) ? 0x04 : 0x03);
+
+    _lidar.read(0x8F, 2, distanceBuffer);
+
+    return ((distanceBuffer[0] << 8) + distanceBuffer[1]) * 10; // 
+  }
+
+  public void reset() {
+    _lidar.write(0x00, 0x00); // Centimeters to meters
+  }
+
+  public LidarConfiguration getCurrentConfiguration() {
+    return LidarConfiguration.values()[config];
+  }
+
+  @Override
+  public void periodic() {
+    double distance = 10 * distance(true);
+    SmartDashboard.putNumber("Lidar Sensor Distance", distance);
+    SmartDashboard.putString("Lidar Config", display[config]);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/LiDARDio.java b/src/main/java/frc/robot/subsystems/LiDARDio.java
new file mode 100644
index 0000000..369ebff
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LiDARDio.java
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class LiDARDio {
+
+  Counter counter;
+
+  public LiDARDio(int port) {
+    counter = new Counter(new DigitalInput(port));
+    counter.setMaxPeriod(1.0);
+    counter.setSemiPeriodMode(true);   
+    reset();
+  }
+
+  public double distance() {
+    return (counter.getPeriod() * 100000000.0) - 18; // TODO: Check if this is in Metres
+  }
+
+  public boolean connected() {
+    return (counter.get() != 0);
+  }
+
+  public void reset() {
+    counter.reset();
+  }
+}
+	
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
new file mode 100644
index 0000000..b0aa046
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.utility.IO;
+
+
+public class Limelight extends SubsystemBase {
+  double area;
+  public double x;
+  double y;
+  double robotyaw;
+  double latency;
+  public int tag; 
+
+  int[] tagLocation;
+
+  final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight");
+  IO io;
+  public Limelight() {
+  }
+
+  @Override
+  public void periodic() {
+    x = table.getEntry("tx").getDouble(0.0);
+    y = table.getEntry("ty").getDouble(0.0);
+    area = table.getEntry("ta").getDouble(0.0);
+    latency =  table.getEntry("tl").getDouble(0.0);
+    tag = (int) table.getEntry("tid").getInteger(0);
+    
+    SmartDashboard.putNumber("LimelightX", x);
+    SmartDashboard.putNumber("LimelightY", y);              //MIGHT NEED TO CHANGE THIS
+    SmartDashboard.putNumber("LimelightArea", area); 
+    SmartDashboard.putNumber("Limelight Latency", latency); 
+  }
+
+  // outputs a pid value to fix the rotation of the robot towards the intended tag
+  // public void fixRotationAndLocation(PIDController pid,double poleX, double poleY) {
+  //   double currentRotation = io.chassis.getYaw();
+  //   double currentPositionX = io.chassis.pose().getX();
+  //   double currentPositionY = io.chassis.pose().getY();
+  //   double tagRotation = TagPose[io.limelight.tag][CENTRE][ROTATION];
+    
+  //   // sets the chassis moving towards the target pole
+  //   io.chassis.drive(new ChassisSpeeds(pid.calculate(currentPositionX, poleX),pid.calculate(currentPositionY, poleY),  pid.calculate(currentRotation, tagRotation)));
+  // }
+  
+  public boolean zoned(){
+    return ((tag > 11) ? Alliance.Blue : Alliance.Red) == DriverStation.getAlliance().get();
+  }
+
+  public boolean reefZone(){
+    return (tag >=6 && tag <= 11 || tag >= 17 && tag <= 22);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
new file mode 100644
index 0000000..a547701
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -0,0 +1,212 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
+import frc.robot.utility.Util;
+
+public class Shooter extends SubsystemBase {
+  
+  SparkMax intake = new SparkMax(13, MotorType.kBrushless);
+  SparkMax pivot = new SparkMax(14, MotorType.kBrushless);
+  public SparkMaxConfig config = new SparkMaxConfig();
+  public SparkMaxConfig pivotConfig = new SparkMaxConfig();
+
+  // DutyCycleEncoder encoder = new DutyCycleEncoder(new DigitalInput(3)); // or 4
+  DoubleSupplier[] position;
+  
+  DigitalInput beam = new DigitalInput(1);
+  int currentCoralSensor = 0;
+  BooleanSupplier[] coral;
+  String[] coralSensingDisplay = {"Beam Break", "RPM"};
+
+  Servo hood = new Servo(0);
+
+  boolean softLimits = false;
+
+  boolean pivotRedundancy = false;
+
+  public static final double[] pivotAngle = (double[]) Util.get("Pivot Angle", new double[] {0,0,0,0,0,0}); // last one FOR BARGE
+  public static final double[] hoodAngle = (double[]) Util.get("Hood Angle", new double[] {0,0,0,0,0,0}); // last one FOR BARGE
+
+  TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500));
+  Timer time = new Timer();
+  double target = 0.0;
+  boolean stopped = true;
+
+  public boolean intaking = false;
+
+  public Shooter() {
+    config.idleMode(SparkMaxConfig.IdleMode.kBrake);
+    intake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+    pivotConfig.idleMode(SparkMaxConfig.IdleMode.kBrake);
+    pivotConfig.closedLoop.pid(0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0);
+
+    pivotConfig.softLimit.forwardSoftLimitEnabled(false);
+    pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit
+    pivotConfig.softLimit.reverseSoftLimitEnabled(false);
+    pivotConfig.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit
+
+    pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+    position = new DoubleSupplier[]{pivot.getEncoder()::getPosition, pivot.getAbsoluteEncoder()::getPosition};
+    coral = new BooleanSupplier[]{beam::get}; //TODO: Have the second option be RPM sensing
+  }
+
+    public void toggleSoftLimits() {
+    softLimits = !softLimits;
+    pivotConfig.softLimit.forwardSoftLimitEnabled(softLimits);
+    pivotConfig.softLimit.reverseSoftLimitEnabled(softLimits);
+    pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+  }
+
+  public void pivotPID(double p, double i, double d){
+    pivotConfig.closedLoop.pid(p, i, d, ClosedLoopSlot.kSlot0);
+    pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+  }
+
+  public void hood(double v){
+    hood.setPosition(v);
+  }
+
+  public void hoodSpeed(double v){
+    hood.setSpeed(v);
+  }
+
+  public void volts(double v) {
+    intake.setVoltage(v);
+  }
+
+  public void speed(double s) {
+    intake.set(s);
+  }
+
+  public void pivotVolts(double v) {
+    pivot.setVoltage(v);
+  }
+
+  public void pivotSpeed(double s) {
+    pivot.set(s);
+  }
+
+  public void stopIntake() {
+    intake.stopMotor();
+  }
+
+  public void stopPivot(){
+    pivot.stopMotor();
+  }
+
+  public void stop(){
+    intake.stopMotor();
+    pivot.stopMotor();
+  }
+
+  public boolean coral() {
+    return coral[currentCoralSensor].getAsBoolean();
+  }
+
+  // public boolean algae() {
+  //   return beam.get();
+  // }
+
+  public Voltage voltage() {
+    return Volts.of(pivot.getBusVoltage());
+  }
+
+  public double angle() {
+    return position[absoluteConnected() ? 1 : 0].getAsDouble();
+  }
+
+  public void angle(double target_angle) {
+    target = target_angle;
+    stopped = false;
+    time.restart();
+  }
+
+  public void angle(int level){
+    angle(pivotAngle[level]);
+    hood.setAngle(hoodAngle[level]);
+  }
+
+  public InstantCommand angleCommand(int level){
+    return new InstantCommand(() -> {
+      angle(pivotAngle[level]);
+      hood.setAngle(hoodAngle[level]);
+    }, this);
+  }
+
+  public void togglePivotRedundancy(){
+    pivotRedundancy = !pivotRedundancy;
+  }
+
+  public boolean absoluteConnected(){
+    return (pivot.getAbsoluteEncoder().getPosition() == -2000); // TODO: 2000 is just a dummy value, replace with actual value present when the cable is not connected
+  }
+
+  public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config(
+      null,
+      Volts.of(4),
+      Seconds.of(5),
+      null),
+      new Mechanism(
+          volts -> pivot.setVoltage(volts),
+          log -> {
+            log.motor("Claw Pivot")
+                .voltage(voltage())
+                .angularPosition(Degree.of(angle()))
+                .angularVelocity(DegreesPerSecond.of(pivot.getAbsoluteEncoder().getVelocity()));
+          }, this));
+
+  @Override
+  public void periodic() {
+    SmartDashboard.putBoolean("Coral", coral());
+    SmartDashboard.putString("Current Coral Sensing", coralSensingDisplay[currentCoralSensor]);
+    
+    SmartDashboard.putNumber("Pivot Angle", angle());
+    SmartDashboard.putNumber("Hood Angle", hood.get() * 180);
+
+    SmartDashboard.putNumber("Pivot Relative Angle", pivot.getEncoder().getPosition());
+    SmartDashboard.putNumber("Pivot Absolute Angle", pivot.getAbsoluteEncoder().getPosition());
+
+    SmartDashboard.putBoolean("Claw Absolute Encoder Connected", absoluteConnected());
+
+    SmartDashboard.putBoolean("Shooter Pivot Soft Limits", softLimits);
+    // TODO: Find out how to indicate if the Hood Servo is connected or not
+
+    double cTime = time.get();
+    if (stopped)
+      return;
+
+    State out = profile.calculate(cTime, new State(angle(), 0), new State(target, 0));
+    pivot.getClosedLoopController().setReference(out.position, ControlType.kPosition);
+    stopped = profile.isFinished(cTime);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
index a4b64c0..22904c2 100644
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -1,43 +1,49 @@
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+
 import com.ctre.phoenix6.hardware.Pigeon2;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+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.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.networktables.StructPublisher;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.modules.KrakenSwerveModule;
-import frc.robot.utility.SwerveConstants;
-import frc.robot.utility.Util;
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import frc.robot.swerve.Module;
+import frc.robot.swerve.Swerve.Constants;
+import frc.robot.utility.LimelightHelpers;
+import frc.robot.utility.Util;
 
 public class Swerve extends SubsystemBase {
 
     public boolean field_oritented = true;
-    public boolean slow_mode = false;
-    private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(
-            new Translation2d(SwerveConstants.TRACKWIDTH / 2.0,
-                    SwerveConstants.WHEELBASE / 2.0),
-            new Translation2d(SwerveConstants.TRACKWIDTH / 2.0,
-                    -SwerveConstants.WHEELBASE / 2.0),
-            new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0,
-                    SwerveConstants.WHEELBASE / 2.0),
-            new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0,
-                    -SwerveConstants.WHEELBASE / 2.0));
-
-    public final Pigeon2 pigeon2 = new Pigeon2(SwerveConstants.PIGEON_ID);
-    public final Timer syncTimer = new Timer();
+    private final SwerveDriveKinematics kinematics;
+
+    public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID);
 
     StructArrayPublisher<SwerveModuleState> current_states = Util.table
             .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish();
@@ -45,161 +51,231 @@ public class Swerve extends SubsystemBase {
             .getStructArrayTopic("Target Module States", SwerveModuleState.struct).publish();
     StructPublisher<Pose2d> posePublisher = NetworkTableInstance.getDefault().getTable("Debug")
             .getStructTopic("Current pose", Pose2d.struct).publish();
-    
+    StructPublisher<Pose2d> estimatedPosePublisher = Util.table
+            .getStructTopic("Estimated Pose", Pose2d.struct).publish();
+
+    SwerveDrivePoseEstimator estimator;
+ 
     final SwerveDriveOdometry odometry;
-    final KrakenSwerveModule[] modules = new KrakenSwerveModule[4];
+    final Module[] modules = new Module[4];
     ChassisSpeeds speeds = new ChassisSpeeds();
+    public final Constants constants = new Constants();
 
     public boolean active = true;
+    boolean encoderRedundancy = false;
+    boolean moduleRedundancy = false;
 
     public Swerve() {
-        new SwerveConstants();
+        kinematics = new SwerveDriveKinematics(
+                createTranslation(constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0),
+                createTranslation(constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0),
+                createTranslation(-constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0),
+                createTranslation(-constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0));
+
         ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain");
         for (int i = 0; i < modules.length; i++) {
-            modules[i] = new KrakenSwerveModule(
-                    tab.getLayout(SwerveConstants.LAYOUT_TITLE[i], BuiltInLayouts.kList)
+            modules[i] = new Module(
+                    tab.getLayout(Constants.LAYOUT_TITLE[i], BuiltInLayouts.kList)
                             .withSize(2, 4)
                             .withPosition(i * 2, 0),
-                    SwerveConstants.CHASSIS_ID[i],
-                    SwerveConstants.CHASSIS_ID[i],
-                    SwerveConstants.ENCODER_ID[i]);
+                    Constants.CHASSIS_ID[i],
+                    Constants.CHASSIS_ID[i],
+                    Constants.ENCODER_ID[i],
+                    constants.comp);
         }
 
         odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(),
                 new Pose2d(0, 0, new Rotation2d()));
-        syncTimer.start();
+
+        estimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), odometry.getPoseMeters());
+
+        AutoBuilder.configure(
+                this::pose,
+                this::resetOdometry,
+                () -> speeds,
+                this::drive,
+                new PPHolonomicDriveController(
+                        new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID
+                        new PIDConstants(constants.ThetaControllerP, 0, constants.ThetaControllerD, 0.0) // Rotation PID
+                ),
+                constants.autoConfig,
+                () -> {
+                    var alliance = DriverStation.getAlliance();
+                    return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red;
+                },
+                this);
     }
 
-    public void zeroGyro() {
-        pigeon2.setYaw(0);
+    private Translation2d createTranslation(double x, double y) {
+        return new Translation2d(x, y);
     }
 
-    public Rotation2d rotation() {
-        return new Rotation2d(absoluteRotation());
+    public void resetAngle() {
+        pigeon2.setYaw(0);
     }
 
-    public double absoluteRotation() {
+    public Rotation2d rotation() {
         double rotation = pigeon2.getYaw().getValueAsDouble() % 360;
         rotation += (rotation < 0) ? 360 : 0;
-        return Math.toRadians(rotation);
+        return new Rotation2d(Degree.of(rotation));
+    }
+
+    public void adjustRotation() {
+        pigeon2.setYaw((rotation().getDegrees() + 180) % 360);
     }
 
     public void drive(ChassisSpeeds speeds) {
         this.speeds = speeds;
     }
 
+    // public void drive(ChassisSpeeds speeds, DriveFeedforwards feedforwards){
+    //     this.speeds = speeds;
+    // }
+
     public void stop() {
         speeds = new ChassisSpeeds();
     }
 
-    public double distance(Pose2d reference_point) {
-        return odometry.getPoseMeters().getTranslation().getDistance(reference_point.getTranslation());
+    public void toggleEncoderRedundancy() {
+        encoderRedundancy = !encoderRedundancy;
     }
 
-    public double distance(double[] reference_point) {
-        return distance(new Pose2d(reference_point[0], reference_point[2], new Rotation2d(reference_point[3])));
+    public void toggleModuleRedundancy() {
+        moduleRedundancy = !moduleRedundancy;
     }
 
-    private SwerveModulePosition modulePosition(KrakenSwerveModule module) {
-        return new SwerveModulePosition(module.drivePosition(), Rotation2d.fromRadians(module.angle()));
-    }
-
-    private SwerveModuleState moduleState(KrakenSwerveModule module) {
-        return new SwerveModuleState(module.velocity(), new Rotation2d(module.angle()));
+    public double distance(Pose2d reference_point) {
+        return pose().getTranslation().getDistance(reference_point.getTranslation());
     }
 
     public SwerveModulePosition[] modulePositions() {
         SwerveModulePosition[] pos = new SwerveModulePosition[4];
         for (int i = 0; i < 4; i++)
-            pos[i] = modulePosition(modules[i]);
+            pos[i] = new SwerveModulePosition(modules[i].drivePosition(), Rotation2d.fromRadians(modules[i].angle()));
         return pos;
     }
 
-    public SwerveModuleState[] moduleStates(KrakenSwerveModule[] modules) {
+    public double getYaw(){
+        return pigeon2.getYaw().getValueAsDouble();
+    }
+
+    public SwerveModuleState[] moduleStates(Module[] modules) {
         SwerveModuleState[] state = new SwerveModuleState[4];
         for (int i = 0; i < modules.length; i++)
-            state[i] = moduleState(modules[i]);
+            state[i] = new SwerveModuleState(modules[i].velocity(), new Rotation2d(modules[i].angle()));
         return state;
     }
 
-    public void adjustRotation() {
-        double rotation = (absoluteRotation() - 180) % 360;
-        rotation += (rotation < 0) ? 360 : 0;
-        pigeon2.setYaw(rotation);
-    }
-
     public Pose2d pose() {
         return odometry.getPoseMeters();
     }
 
     public void resetOdometry() {
-        setOdometry(new Pose2d());
+        resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(0)));
     }
 
     public void resetOdometry(Pose2d pose) {
-        pigeon2.setYaw(0);
-        resetPosition();
+        pigeon2.setYaw(pose.getRotation().getDegrees());
+        resetModulePositions();
 
         odometry.resetPosition(rotation(), modulePositions(), pose);
-        odometry.resetPosition(rotation(), modulePositions(), pose);
     }
 
-    public void setOdometry(Pose2d pose) {
-        zeroGyro();
-        resetPosition();
-
-        odometry.resetPosition(rotation(), modulePositions(), pose);
-        odometry.resetPosition(rotation(), modulePositions(), pose);
-    }
-
-    public void resetPosition() {
-        for (KrakenSwerveModule mod : modules)
+    public void resetModulePositions() {
+        for (Module mod : modules)
             mod.resetDrivePosition();
     }
 
     public void syncEncoders() {
-        for (KrakenSwerveModule mod : modules)
-            mod.syncSteerEncoders();
-    }
-
-    public void resetAbsolute() {
-        for (KrakenSwerveModule mod : modules)
-            mod.resetAbsolute();
+        for (Module mod : modules)
+            mod.syncEncoders();
     }
 
-    public void resetSteerPositions() {
-        for (KrakenSwerveModule mod : modules)
-            mod.set(0, 0);
+    public void zeroAbsolute() {
+        for (Module mod : modules)
+            mod.zeroAbsolute();
     }
 
-    public void setModuleStates(SwerveModuleState[] states) { 
-        SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_VELOCITY);
+    public void setModuleStates(SwerveModuleState[] states) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.MAX_VELOCITY);
         for (int i = 0; i < modules.length; i++) {
             states[i].optimize(new Rotation2d(modules[i].angle()));
-            // states[i].cosineScale(new Rotation2d(modules[i].angle())); // Cosine Compensation
-            modules[i].set((states[i].speedMetersPerSecond / SwerveConstants.MAX_VELOCITY) * .8 , states[i].angle.getRadians());
+            modules[i].set((states[i].speedMetersPerSecond / Constants.MAX_VELOCITY) * .8,
+                    states[i].angle.getRadians());
         }
     }
 
-    public ChassisSpeeds getSpeeds() {
-        return speeds;
-    }
+    final MutDistance[] distance = { Meters.mutable(0), Meters.mutable(0), Meters.mutable(0), Meters.mutable(0) };
+    final MutAngle[] angle = { Radians.mutable(0), Radians.mutable(0), Radians.mutable(0), Radians.mutable(0) };
+
+    public final SysIdRoutine driveRoutine = new SysIdRoutine(new Config(
+            null,
+            Volts.of(3),
+            // Seconds.of(5),
+            null,
+            null),
+            new SysIdRoutine.Mechanism(voltage -> {
+                for (Module mod : modules)
+                    mod.set(voltage.magnitude(), 0);
+            }, log -> {
+                    log.motor(Constants.LAYOUT_TITLE[0] + " [Drive]")
+                            .voltage(modules[0].voltage())
+                            .linearPosition(distance[0].mut_replace(modules[0].drivePosition(), Meters))
+                            .linearVelocity(modules[0].velocity())
+                            .linearAcceleration(pigeon2.getAccelerationX().getValue()); // NOTE: This is due to our gyro being mounted 90 degrees off centre
+            }, this));
+
+    public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config(
+            null,
+            Volts.of(2),
+            null,
+            null),
+            new SysIdRoutine.Mechanism(voltage -> {
+                speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_ANGULAR_VELOCITY);
+            }, log -> {
+                log.motor("Chassis")
+                    .voltage(modules[0].steerVoltage())
+                    .angularPosition(pigeon2.getYaw().getValue())
+                    .angularVelocity(pigeon2.getAngularVelocityYWorld().getValue());
+                    // .angularAcceleration(pigeon2.getAccelerationY().getValue()); // TODO: Test to see if converting this to angular acceleration would be valid
+
+                // for (int i = 0; i < 4; i++) {
+                //     log.motor(Constants.LAYOUT_TITLE[i] + " [Steer]")
+                //             .voltage(modules[i].steerVoltage())
+                //             .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians))
+                //             .angularVelocity(modules[i].steerVelocity());
+                // }
+            }, this));
 
     public double getRoll() {
         return pigeon2.getRoll().getValueAsDouble();
     }
 
-    public void enable() {
-        active = true;
-    }
-
-    public void disable() {
-        active = false;
+    public void toggle() {
+        active = !active;
 
-        for (KrakenSwerveModule mod : modules)
+        for (Module mod : modules)
             mod.stop();
     }
 
+    public Pose2d estimatePose() {
+        estimator.update(rotation(), modulePositions());
+        // double tagAngle = (LimelightHelpers.getTargetPose_CameraSpace("limelight-main")[3] + 360) %360;
+        // double ourPos =  tagAngle - LimelightHelpers.getTX("limelight-main")  + 130;
+        // SmartDashboard.putNumber("Our Angle", ourPos);
+        LimelightHelpers.SetRobotOrientation("limelight-main", 0, 0,0,0,0,0);
+        LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main");
+        
+        
+        if (mt2 == null) return new Pose2d();
+        else if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){
+            estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999));
+            estimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds);
+            estimatedPosePublisher.set(estimator.getEstimatedPosition());
+        }
+        return mt2.pose;
+    }
+
     public void periodic() {
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
         if (active && speeds != new ChassisSpeeds())
@@ -208,8 +284,8 @@ public void periodic() {
         current_states.set(moduleStates(modules));
         target_states.set(states);
 
-
         Pose2d pose = odometry.update(rotation(), modulePositions());
+        estimatePose();
         posePublisher.set(pose);
 
         SmartDashboard.putNumber("X position", pose.getX());
@@ -217,9 +293,24 @@ public void periodic() {
 
         SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees());
         SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble());
-        SmartDashboard.putNumber("Pigeon Pitch", pigeon2.getPitch().getValueAsDouble());
-        SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble());
+        SmartDashboard.putNumber("Pigeon Pitch",
+        pigeon2.getPitch().getValueAsDouble());
+        SmartDashboard.putNumber("Pigeon Roll",
+        pigeon2.getRoll().getValueAsDouble());
 
         SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented");
+
+        // DogLog.log("Swerve/Current States", moduleStates(modules));
+        // DogLog.log("Swerve/Target States", states);
+
+        // DogLog.log("Swerve/X Position", pose.getX());
+        // DogLog.log("Swerve/Y Position", pose.getY());
+        // DogLog.log("Swerve/Pose", pose);
+        // DogLog.log("Swerve/Odometry Rotation", rotation().getDegrees());
+
+        // DogLog.log("Swerve/Pigeon Yaw", getYaw());
+        // DogLog.log("Swerve/Pigeon Pitch", pigeon2.getPitch().getValueAsDouble());
+        // DogLog.log("Swerve/Pigeon Roll", pigeon2.getRoll().getValueAsDouble());
+        // DogLog.log("Swerve/Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented");
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java
new file mode 100644
index 0000000..70c01dd
--- /dev/null
+++ b/src/main/java/frc/robot/swerve/Module.java
@@ -0,0 +1,140 @@
+package frc.robot.swerve;
+
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.SparkMax;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+
+public class Module {
+    public final TalonFX drive;
+    public final SparkMax steer;
+    public final Swerve.Encoder encoder;
+
+    double desiredAngle;
+
+    public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
+    public static final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0);
+
+    public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, boolean comp) {
+        drive = new TalonFX(driveID, "rio");
+        steer = new SparkMax(steerID, MotorType.kBrushless);
+        encoder = (comp) ? new Swerve.Canand(encoderID) : new Swerve.Cancoder(encoderID);
+
+        SparkMaxConfig steerConfig = new SparkMaxConfig();
+
+        steerConfig
+                .smartCurrentLimit(20)
+                .idleMode(IdleMode.kBrake)
+                .inverted(true);
+
+        steerConfig.encoder
+                .positionConversionFactor(Math.PI * STEER_REDUCTION)
+                .velocityConversionFactor(Math.PI * STEER_REDUCTION / 60);
+
+        steerConfig.closedLoop
+                .positionWrappingEnabled(true)
+                .positionWrappingMaxInput(Swerve.PI2)
+                // .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+                .pid(0.2, 0.0, 0.0);
+
+                steerConfig.signals.primaryEncoderPositionAlwaysOn(false);
+                steerConfig.signals.primaryEncoderPositionPeriodMs(20);
+
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        config.CurrentLimits.StatorCurrentLimit = 80;
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+
+        config.CurrentLimits.SupplyCurrentLimit = 20;
+        config.CurrentLimits.SupplyCurrentLimitEnable = true;
+        
+        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+        config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        steer.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        steer.getEncoder().setPosition(angle());
+
+        drive.getConfigurator().apply(config);
+
+        tab.addDouble("Absolute Angle", () -> Math.toDegrees(angle()));
+        tab.addDouble("Current Angle", () -> Math.toDegrees(steer.getEncoder().getPosition()));
+        tab.addDouble("Angle Difference", () -> Math.toDegrees(angle() - steer.getEncoder().getPosition()));
+        tab.addDouble("Target Angle", () -> Math.toDegrees(desiredAngle));
+        tab.addBoolean("Active", encoder::connected);
+    }
+
+    public void resetDrivePosition() {
+        drive.setPosition(0.0);
+    }
+
+    public void syncEncoders() {
+        steer.getEncoder().setPosition(encoder.angle());
+    }
+
+    public void zeroAbsolute() {
+        encoder.zero();
+    }
+
+    public double drivePosition() {
+        return drive.getPosition().getValueAsDouble() * .632 * WHEEL_DIAMETER;
+    }
+
+    public LinearVelocity velocity() {
+        return MetersPerSecond.of(drive.getVelocity().getValueAsDouble() * Swerve.PI2 * .632 * WHEEL_DIAMETER);
+    }
+
+    public Voltage voltage(){
+        return drive.getMotorVoltage().getValue();
+    }
+
+    public double angle() {
+        return encoder.angle();
+    }
+
+    public AngularVelocity steerVelocity(){
+        return encoder.velocity();
+    }
+
+    public Voltage steerVoltage(){
+        return Volts.of(steer.getBusVoltage());
+    }
+
+    public void stop() {
+        drive.stopMotor();
+        steer.stopMotor();
+    }
+
+    public void set(double driveVolts, double targetAngle) {
+        syncEncoders();
+
+        drive.set(driveVolts);
+        steer.getClosedLoopController().setReference(targetAngle, ControlType.kPosition);
+    }
+
+    public void setSteer(double steerVolts){
+        syncEncoders();
+        drive.set(0);
+        steer.set(steerVolts);
+    }
+
+}
diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java
new file mode 100644
index 0000000..1b97854
--- /dev/null
+++ b/src/main/java/frc/robot/swerve/Swerve.java
@@ -0,0 +1,148 @@
+    package frc.robot.swerve;
+
+    import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+    import com.ctre.phoenix6.hardware.CANcoder;
+    import com.ctre.phoenix6.signals.SensorDirectionValue;
+    import com.pathplanner.lib.config.RobotConfig;
+    import com.reduxrobotics.sensors.canandmag.Canandmag;
+    import com.reduxrobotics.sensors.canandmag.CanandmagSettings;
+
+    import edu.wpi.first.units.measure.AngularVelocity;
+
+    import static edu.wpi.first.units.Units.Radians;
+    import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+    import java.io.IOException;
+
+    public class Swerve {
+
+        public static final double PI2 = 2.0 * Math.PI;
+
+        public static class Constants {
+            // BOT SWITCHING
+            public boolean comp = true;
+
+            public double TRACKWIDTH = 30; // 30.0 for MKi
+            public double WHEELBASE = 30; // 30.0 for MKi
+            public double GEAR_RATIO = 8.14;
+            public double WHEEL_RADIUS = .1143;
+
+            // DRIVER SETTINGS
+            public static int driver = 0;
+            public static double transFactor = 1.0; // factor = x/125, with x being the percentage of our max speed, same for the thing below
+            public static double rotFactor = .30; // .6 for tristan
+
+            // AUTON CONSTANTS
+            public double XControllerP = 1.6878;
+            public double XControllerD = 0;
+            public double ThetaControllerP = 0;
+            public double ThetaControllerD = 0;
+            public RobotConfig autoConfig;
+
+            // BASE CHASSIS CONFIGURATION
+            public static final double MAX_VELOCITY = 5.4;
+            public static final double MAX_ANGULAR_VELOCITY = Math.PI/6;
+            public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" };
+            public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR
+            public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR
+            public static double[] ENCODER_OFFSETS = {-0.87890625, -0.996337890625, -0.638427734375, -0.892822265625};
+            public static final int PIGEON_ID = 6;
+
+            public Constants(){
+                try {
+                    autoConfig = RobotConfig.fromGUISettings();
+                } catch (IOException | org.json.simple.parser.ParseException e) {
+                    e.printStackTrace();
+                }
+                SwitchDriver(driver);
+            }
+
+            public static void SwitchDriver(int driver){
+                switch (driver) {
+                    case 4: // DEBUG
+                    break;
+
+                    case 3: // Uriel
+                    break;
+
+                    case 2: // Jason
+                    break;
+                    
+                    case 1: // Norah
+                    break;
+                    
+                    default: // Shaan is a king
+                    transFactor = 1.0;
+                    rotFactor = .5;
+                        break;
+                }
+            }
+        }
+
+        public interface Encoder {
+            public void zero();
+            public boolean connected();
+            public double angle();
+            public AngularVelocity velocity();
+
+        }
+
+        public static class Cancoder implements Encoder {
+
+            CANcoder encoder;
+            MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs();
+
+            public Cancoder(int id) {
+                encoder = new CANcoder(id);
+                magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+                magnetConfig.AbsoluteSensorDiscontinuityPoint = 1.0;
+                magnetConfig.withMagnetOffset(Swerve.Constants.ENCODER_OFFSETS[id-7]);
+                encoder.getConfigurator().apply(magnetConfig);
+            }
+
+            public void zero() {
+                magnetConfig.MagnetOffset = -encoder.getAbsolutePosition().getValueAsDouble();
+                encoder.getConfigurator().apply(magnetConfig);
+            }
+
+            public boolean connected(){
+                return encoder.isConnected();
+            }
+
+            public double angle() {
+                return ((encoder.getAbsolutePosition().getValue().in(Radians) + PI2) % PI2);
+            }
+
+            public AngularVelocity velocity(){
+                return encoder.getVelocity().getValue();
+            }
+        }
+
+        public static class Canand implements Encoder {
+            Canandmag encoder;
+
+            public Canand(int id) {
+                encoder = new Canandmag(id);
+                CanandmagSettings settings = new CanandmagSettings();
+                settings.setInvertDirection(true);
+                encoder.clearStickyFaults();
+                encoder.setSettings(settings);
+            }
+
+            public void zero() {
+                encoder.setAbsPosition(0, 250);
+            }
+
+            public boolean connected(){
+                return encoder.isConnected();
+            }
+
+            public double angle() {
+                return (encoder.getAbsPosition() * PI2) % PI2;
+            }
+
+            public AngularVelocity velocity(){
+                return RadiansPerSecond.of(encoder.getVelocity());
+            }
+        }
+    }
diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java
index 0805aef..10d080c 100644
--- a/src/main/java/frc/robot/utility/AutomatedController.java
+++ b/src/main/java/frc/robot/utility/AutomatedController.java
@@ -1,110 +1,158 @@
 package frc.robot.utility;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.IntSupplier;
 
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.GrabAlgae;
-import frc.robot.commands.ReleaseAlgae;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.commands.Intake;
+import frc.robot.commands.ScoreReef;
 
 public class AutomatedController {
     public final CommandXboxController controller;
     public final SendableChooser<Runnable> selector = new SendableChooser<Runnable>();
-    public boolean manual = true;
+    public int mode = 3;
+    public GenericHID rumble = new GenericHID(0);
 
     IO io;
 
     public AutomatedController(int port, IO io){
         this.io = io;
 
-        selector.setDefaultOption("Automated", () -> {manual = false;});
-        selector.addOption("Manual", () -> {manual = true;});
+        selector.setDefaultOption("Automated", () -> {mode = 0;});
+        selector.addOption("Manual", () -> {mode = 1;});
+        selector.addOption("Characterise", () -> {mode = 2;});
+        selector.addOption("Debug", () -> {mode = 3;});
 
-        selector.onChange((x) -> {x.run();}); // TODO: See if this allows us to have the selector always be up-to-date and add the same for auton testing
+        selector.onChange((x) -> {x.run();});
 
         controller = new CommandXboxController(port);
+        // rumble = new Rumble(controller.getHID());
+
         controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented));
-        controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry));
+        controller.leftStick().debounce(2).onTrue(new InstantCommand(io.chassis::resetAngle));
+        configure();
         // controller.back().onTrue(Util.Do(io.elevator::rest));
         // controller.start().onTrue(Util.Do(io.elevator::zero));
-        controller.start().onTrue(Util.Do( () -> manual = !manual)).debounce(3);
-        configure();
+
     }
-    public BooleanSupplier automated(){
-        return () -> { return !manual; };
+
+    public BooleanSupplier mode(int targetMode){
+        return () -> {return mode == targetMode;};
     }
 
+    public BooleanSupplier automated(){
+        return mode(0);
+    }
+    
     public BooleanSupplier manual(){
-        return () -> { return manual; };
+        return mode(1);
+    }
+
+    public BooleanSupplier characterise(){
+        return mode(2);
     }
 
-    public BooleanSupplier toggleMode(){
-        return () -> {
-            manual = !manual;
-            return manual;
-        };
+    public BooleanSupplier debug(){
+        return mode(3);
+    }
+
+    public void switchMode(){
+        mode = (mode + 1) % 3;
     }
 
     public void configure(){
-        
-        controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::toggleMode));
-        controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis));
+        controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode));
+        configureAutomated();
+        configureManual();
+        configureCharacterisaton();
+        configureDebug();
+    }
 
-        // AUTOMATED
+    void configureAutomated(){
 
-        // Based on the nearest element and our field orientation
-        // LB align Left and Score Coral & Score Barge
-        // RB align Right and Score Coral & Score Processor 
+        IntSupplier pos = () -> { return ((controller.getHID().getLeftBumperButtonPressed()) ? -1 : 0) + ((controller.getHID().getLeftBumperButtonPressed()) ? 1 : 0) + 1; };
 
-        controller.y().and( automated() ).onTrue(Util.Do(io.elevator::L4));
-        controller.b().and( automated() ).onTrue(Util.Do(io.elevator::L3));
-        controller.a().and( automated() ).onTrue(Util.Do(io.elevator::L2));
-        controller.x().and( automated() )
-            .onTrue(Util.Do(
-                new ReleaseAlgae(io, false),
-                new GrabAlgae(io),
-                io.intake::grabbed));
+        controller.y().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4)));
+        controller.x().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3)));
+        controller.b().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2)));
+        controller.a().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1)));
+        controller.a().and(automated()).and(() -> !io.shooter.coral()).onTrue(new Intake(io, false));
 
-        controller.povUp().and( automated() )
-            .onTrue(Util.Do(() -> io.elevator.speed(0.25)))
-            .onFalse(Util.Do(() -> io.elevator.speed(0.0)));
+        controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0)));
+        controller.back().onTrue(Util.Do(io.chassis::resetAngle, io.chassis));
+    }
 
-        controller.povDown().and( automated() )
-            .onTrue(Util.Do(() -> io.elevator.speed(-0.25)))
-            .onFalse(Util.Do(() -> io.elevator.speed(0.0)));
+    double direction = 1; 
 
-        controller.povLeft().and( automated() )
-            .onTrue(Util.Do(() -> io.intake.speed(-0.5)))
-            .onFalse(Util.Do(() -> io.intake.speed(0.0)));
+    public void configureManual(){
+        controller.back().and(manual()).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis));
 
-        controller.povRight().and( automated() )
-            .onTrue(Util.Do(() -> io.intake.speed(0.5)))
-            .onFalse(Util.Do(() -> io.intake.speed(0.0)));
+        // controller.y().and(manual()).onTrue(Util.Do(() -> io.elevator.move(4)));
+        controller.x().and(manual()).onTrue(Util.Do(() -> io.elevator.move(3)));
+        controller.b().and(manual()).onTrue(Util.Do(() -> io.elevator.move(2)));
+        controller.a().and(manual()).onTrue(Util.Do(() -> io.elevator.move(1)));
 
-            // MANUAL
+        controller.leftBumper().onTrue(Util.Do(() -> io.shooter.speed(direction * .4), io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter));
+        controller.rightBumper().onTrue(Util.Do(() -> io.shooter.speed(direction * 1), io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter));
+        controller.y().and(manual()).onTrue(Util.Do(() -> { direction = -direction;}));
+        
+        // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle));
+        // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders));
+        // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect
+    }
 
-            controller.rightTrigger().and( manual() )
-            .onTrue(Util.Do(() -> io.elevator.speed(0.25)))
-            .onFalse(Util.Do(() -> io.elevator.speed(0.0)));
+    void configureCharacterisaton(){
+ 
+        // controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward));
+        // controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse));
+        // controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward));
+        // controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse));
 
-        controller.leftTrigger().and( manual() )
-            .onTrue(Util.Do(() -> io.elevator.speed(-0.25)))
-            .onFalse(Util.Do(() -> io.elevator.speed(0.0)));
+        // controller.povUp().and(characterise()).toggleOnTrue(io.chassis.   steerRoutine.quasistatic(Direction.kForward));
+        // controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse));
+        // controller.povRight().and(characterise()).toggleOnTrue(io.steerRoutine.dynamic(Direction.kForward));
+        // controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse));
 
-        controller.leftBumper().and( manual() )
-            .onTrue(Util.Do(() -> io.intake.speed(-0.5)))
-            .onFalse(Util.Do(() -> io.intake.speed(0.0)));
+        controller.x().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kForward));
+        controller.a().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kReverse));
+        controller.y().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kForward));
+        controller.b().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kReverse));
 
-        controller.rightBumper().and( manual() )
-            .onTrue(Util.Do(() -> io.intake.speed(0.5)))
-            .onFalse(Util.Do(() -> io.intake.speed(0.0)));
+        controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward));
+        controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse));
+        controller.leftTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward));
+        controller.rightTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse));
 
-        // controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable));
-        // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5);
-        // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders));
-        // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::resetAbsolute)); // Add the Rumble effect
+    }
 
-        // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::resetAbsolute)); // Add the Rumble effect
+    void configureDebug(){
+        controller.back().and(debug()).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis));
+
+        double volts = 3;
+
+        controller.leftBumper().and(debug()).onTrue(Util.Do(() -> {
+            io.elevator.volts(-volts / 2);
+            io.elevator.stopped = true;
+        }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator));
+        controller.rightBumper().and(debug()).onTrue(Util.Do(() -> {
+            io.elevator.volts(volts);
+            io.elevator.stopped = true;
+        }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator));
+
+        controller.y().and(debug()).onTrue(Util.Do(() -> {
+            io.shooter.volts(volts);
+        }, io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter));
+
+
+        controller.x().and(debug()).onTrue(Util.Do(io.elevator::zero, io.elevator));
+        controller.a().and(debug()).onTrue(Util.Do(io.elevator::toggleSoftLimits, io.elevator));
+         controller.povLeft().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator));
+        controller.povRight().and(debug()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator));
+        controller.povDown().and(debug()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator));
     }
+
 }
diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java
index 29f1c26..5180860 100644
--- a/src/main/java/frc/robot/utility/IO.java
+++ b/src/main/java/frc/robot/utility/IO.java
@@ -6,14 +6,14 @@
 
 public class IO extends SubsystemBase {
         public final Swerve chassis = new Swerve();
-        public final AlgaeIntake intake = new AlgaeIntake();
         public final Elevator elevator = new Elevator();
-        public final Hang hang = new Hang();
+        public final Limelight limelight = new Limelight();
 
-        public CommandScheduler scheduler = CommandScheduler.getInstance();
+        public final Shooter shooter = new Shooter();
+        public final Hang hang = null;
 
         public IO() {
-                DriverStation.silenceJoystickConnectionWarning(true);
+           DriverStation.silenceJoystickConnectionWarning(true);
         }
 
         @Override
diff --git a/src/main/java/frc/robot/utility/LimelightHelpers.java b/src/main/java/frc/robot/utility/LimelightHelpers.java
new file mode 100644
index 0000000..4732992
--- /dev/null
+++ b/src/main/java/frc/robot/utility/LimelightHelpers.java
@@ -0,0 +1,1587 @@
+//LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER)
+
+package frc.robot.utility;
+
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.TimestampedDoubleArray;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.io.IOException;
+import java.net.HttpURLConnection;
+import java.net.MalformedURLException;
+import java.net.URL;
+import java.util.Map;
+import java.util.concurrent.CompletableFuture;
+
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import java.util.concurrent.ConcurrentHashMap;
+
+/**
+ * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC.
+ * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking.
+ */
+public class LimelightHelpers {
+
+    private static final Map<String, DoubleArrayEntry> doubleArrayEntries = new ConcurrentHashMap<>();
+
+    /**
+     * Represents a Color/Retroreflective Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Retro {
+
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private  double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+        
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ts")
+        public double ts;
+
+        public LimelightTarget_Retro() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+
+    }
+
+    /**
+     * Represents an AprilTag/Fiducial Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Fiducial {
+
+        @JsonProperty("fID")
+        public double fiducialID;
+
+        @JsonProperty("fam")
+        public String fiducialFamily;
+
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+        
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ts")
+        public double ts;
+        
+        public LimelightTarget_Fiducial() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+    }
+
+    /**
+     * Represents a Barcode Target Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Barcode {
+
+        /**
+         * Barcode family type (e.g. "QR", "DataMatrix", etc.)
+         */
+        @JsonProperty("fam")
+        public String family;
+
+        /**
+         * Gets the decoded data content of the barcode
+         */
+        @JsonProperty("data") 
+        public String data;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("pts")
+        public double[][] corners;
+
+        public LimelightTarget_Barcode() {
+        }
+
+        public String getFamily() {
+            return family;
+        }
+    }
+
+    /**
+     * Represents a Neural Classifier Pipeline Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Classifier {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("zone")
+        public double zone;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        public  LimelightTarget_Classifier() {
+        }
+    }
+
+    /**
+     * Represents a Neural Detector Pipeline Result extracted from JSON Output
+     */
+    public static class LimelightTarget_Detector {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        public LimelightTarget_Detector() {
+        }
+    }
+
+    /**
+     * Limelight Results object, parsed from a Limelight's JSON results output.
+     */
+    public static class LimelightResults {
+        
+        public String error;
+        
+        @JsonProperty("pID")
+        public double pipelineID;
+
+        @JsonProperty("tl")
+        public double latency_pipeline;
+
+        @JsonProperty("cl")
+        public double latency_capture;
+
+        public double latency_jsonParse;
+
+        @JsonProperty("ts")
+        public double timestamp_LIMELIGHT_publish;
+
+        @JsonProperty("ts_rio")
+        public double timestamp_RIOFPGA_capture;
+
+        @JsonProperty("v")
+        @JsonFormat(shape = Shape.NUMBER)
+        public boolean valid;
+
+        @JsonProperty("botpose")
+        public double[] botpose;
+
+        @JsonProperty("botpose_wpired")
+        public double[] botpose_wpired;
+
+        @JsonProperty("botpose_wpiblue")
+        public double[] botpose_wpiblue;
+
+        @JsonProperty("botpose_tagcount")
+        public double botpose_tagcount;
+       
+        @JsonProperty("botpose_span")
+        public double botpose_span;
+       
+        @JsonProperty("botpose_avgdist")
+        public double botpose_avgdist;
+       
+        @JsonProperty("botpose_avgarea")
+        public double botpose_avgarea;
+
+        @JsonProperty("t6c_rs")
+        public double[] camerapose_robotspace;
+
+        public Pose3d getBotPose3d() {
+            return toPose3D(botpose);
+        }
+    
+        public Pose3d getBotPose3d_wpiRed() {
+            return toPose3D(botpose_wpired);
+        }
+    
+        public Pose3d getBotPose3d_wpiBlue() {
+            return toPose3D(botpose_wpiblue);
+        }
+
+        public Pose2d getBotPose2d() {
+            return toPose2D(botpose);
+        }
+    
+        public Pose2d getBotPose2d_wpiRed() {
+            return toPose2D(botpose_wpired);
+        }
+    
+        public Pose2d getBotPose2d_wpiBlue() {
+            return toPose2D(botpose_wpiblue);
+        }
+
+        @JsonProperty("Retro")
+        public LimelightTarget_Retro[] targets_Retro;
+
+        @JsonProperty("Fiducial")
+        public LimelightTarget_Fiducial[] targets_Fiducials;
+
+        @JsonProperty("Classifier")
+        public LimelightTarget_Classifier[] targets_Classifier;
+
+        @JsonProperty("Detector")
+        public LimelightTarget_Detector[] targets_Detector;
+
+        @JsonProperty("Barcode")
+        public LimelightTarget_Barcode[] targets_Barcode;
+
+        public LimelightResults() {
+            botpose = new double[6];
+            botpose_wpired = new double[6];
+            botpose_wpiblue = new double[6];
+            camerapose_robotspace = new double[6];
+            targets_Retro = new LimelightTarget_Retro[0];
+            targets_Fiducials = new LimelightTarget_Fiducial[0];
+            targets_Classifier = new LimelightTarget_Classifier[0];
+            targets_Detector = new LimelightTarget_Detector[0];
+            targets_Barcode = new LimelightTarget_Barcode[0];
+
+        }
+
+
+    }
+
+    /**
+     * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output.
+     */
+    public static class RawFiducial {
+        public int id = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double distToCamera = 0;
+        public double distToRobot = 0;
+        public double ambiguity = 0;
+
+
+        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
+            this.id = id;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.distToCamera = distToCamera;
+            this.distToRobot = distToRobot;
+            this.ambiguity = ambiguity;
+        }
+    }
+
+    /**
+     * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output.
+     */
+    public static class RawDetection {
+        public int classId = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double corner0_X = 0;
+        public double corner0_Y = 0;
+        public double corner1_X = 0;
+        public double corner1_Y = 0;
+        public double corner2_X = 0;
+        public double corner2_Y = 0;
+        public double corner3_X = 0;
+        public double corner3_Y = 0;
+
+
+        public RawDetection(int classId, double txnc, double tync, double ta, 
+            double corner0_X, double corner0_Y, 
+            double corner1_X, double corner1_Y, 
+            double corner2_X, double corner2_Y, 
+            double corner3_X, double corner3_Y ) {
+            this.classId = classId;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.corner0_X = corner0_X;
+            this.corner0_Y = corner0_Y;
+            this.corner1_X = corner1_X;
+            this.corner1_Y = corner1_Y;
+            this.corner2_X = corner2_X;
+            this.corner2_Y = corner2_Y;
+            this.corner3_X = corner3_X;
+            this.corner3_Y = corner3_Y;
+        }
+    }
+    
+    /**
+     * Represents a 3D Pose Estimate.
+     */
+    public static class PoseEstimate {
+        public Pose2d pose;
+        public double timestampSeconds;
+        public double latency;
+        public int tagCount;
+        public double tagSpan;
+        public double avgTagDist;
+        public double avgTagArea;
+
+        public RawFiducial[] rawFiducials; 
+        public boolean isMegaTag2;
+
+        /**
+         * Instantiates a PoseEstimate object with default values
+         */
+        public PoseEstimate() {
+            this.pose = new Pose2d();
+            this.timestampSeconds = 0;
+            this.latency = 0;
+            this.tagCount = 0;
+            this.tagSpan = 0;
+            this.avgTagDist = 0;
+            this.avgTagArea = 0;
+            this.rawFiducials = new RawFiducial[]{};
+            this.isMegaTag2 = false;
+        }
+
+        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
+            int tagCount, double tagSpan, double avgTagDist, 
+            double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) {
+
+            this.pose = pose;
+            this.timestampSeconds = timestampSeconds;
+            this.latency = latency;
+            this.tagCount = tagCount;
+            this.tagSpan = tagSpan;
+            this.avgTagDist = avgTagDist;
+            this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
+            this.isMegaTag2 = isMegaTag2;
+        }
+
+    }
+
+    private static ObjectMapper mapper;
+
+    /**
+     * Print JSON Parse time to the console in milliseconds
+     */
+    static boolean profileJSON = false;
+
+    static final String sanitizeName(String name) {
+        if (name == "" || name == null) {
+            return "limelight";
+        }
+        return name;
+    }
+
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose3d object.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose3d object representing the pose, or empty Pose3d if invalid data
+     */
+    public static Pose3d toPose3D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 3D Pose Data!");
+            return new Pose3d();
+        }
+        return new Pose3d(
+            new Translation3d(inData[0], inData[1], inData[2]),
+            new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]),
+                    Units.degreesToRadians(inData[5])));
+    }
+
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose2d object.
+     * Uses only x, y, and yaw components, ignoring z, roll, and pitch.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose2d object representing the pose, or empty Pose2d if invalid data
+     */
+    public static Pose2d toPose2D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 2D Pose Data!");
+            return new Pose2d();
+        }
+        Translation2d tran2d = new Translation2d(inData[0], inData[1]);
+        Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
+        return new Pose2d(tran2d, r2d);
+    }
+
+    /**
+     * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * 
+     * @param pose The Pose3d object to convert
+     * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
+     */
+    public static double[] pose3dToArray(Pose3d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = pose.getTranslation().getZ();
+        result[3] = Units.radiansToDegrees(pose.getRotation().getX());
+        result[4] = Units.radiansToDegrees(pose.getRotation().getY());
+        result[5] = Units.radiansToDegrees(pose.getRotation().getZ());
+        return result;
+    }
+
+    /**
+     * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw.
+     * 
+     * @param pose The Pose2d object to convert
+     * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
+     */
+    public static double[] pose2dToArray(Pose2d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = 0;
+        result[3] = Units.radiansToDegrees(0);
+        result[4] = Units.radiansToDegrees(0);
+        result[5] = Units.radiansToDegrees(pose.getRotation().getRadians());
+        return result;
+    }
+
+    private static double extractArrayEntry(double[] inData, int position){
+        if(inData.length < position+1)
+        {
+            return 0;
+        }
+        return inData[position];
+    }
+
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) {
+        DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
+        
+        TimestampedDoubleArray tsValue = poseEntry.getAtomic();
+        double[] poseArray = tsValue.value;
+        long timestamp = tsValue.timestamp;
+        
+        if (poseArray.length == 0) {
+            // Handle the case where no data is available
+            return null; // or some default PoseEstimate
+        }
+    
+        var pose = toPose2D(poseArray);
+        double latency = extractArrayEntry(poseArray, 6);
+        int tagCount = (int)extractArrayEntry(poseArray, 7);
+        double tagSpan = extractArrayEntry(poseArray, 8);
+        double tagDist = extractArrayEntry(poseArray, 9);
+        double tagArea = extractArrayEntry(poseArray, 10);
+        
+        // Convert server timestamp from microseconds to seconds and adjust for latency
+        double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0);
+    
+        RawFiducial[] rawFiducials = new RawFiducial[tagCount];
+        int valsPerFiducial = 7;
+        int expectedTotalVals = 11 + valsPerFiducial * tagCount;
+    
+        if (poseArray.length != expectedTotalVals) {
+            // Don't populate fiducials
+        } else {
+            for(int i = 0; i < tagCount; i++) {
+                int baseIndex = 11 + (i * valsPerFiducial);
+                int id = (int)poseArray[baseIndex];
+                double txnc = poseArray[baseIndex + 1];
+                double tync = poseArray[baseIndex + 2];
+                double ta = poseArray[baseIndex + 3];
+                double distToCamera = poseArray[baseIndex + 4];
+                double distToRobot = poseArray[baseIndex + 5];
+                double ambiguity = poseArray[baseIndex + 6];
+                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+            }
+        }
+    
+        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2);
+    }
+
+    /**
+     * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawFiducial objects containing detection details
+     */
+    public static RawFiducial[] getRawFiducials(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
+        var rawFiducialArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 7;
+        if (rawFiducialArray.length % valsPerEntry != 0) {
+            return new RawFiducial[0];
+        }
+    
+        int numFiducials = rawFiducialArray.length / valsPerEntry;
+        RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
+    
+        for (int i = 0; i < numFiducials; i++) {
+            int baseIndex = i * valsPerEntry;
+            int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
+            double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
+            double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
+            double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
+            double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
+            
+            rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+        }
+    
+        return rawFiducials;
+    }
+
+    /**
+     * Gets the latest raw neural detector results from NetworkTables
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawDetection objects containing detection details
+     */
+    public static RawDetection[] getRawDetections(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
+        var rawDetectionArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 12;
+        if (rawDetectionArray.length % valsPerEntry != 0) {
+            return new RawDetection[0];
+        }
+    
+        int numDetections = rawDetectionArray.length / valsPerEntry;
+        RawDetection[] rawDetections = new RawDetection[numDetections];
+    
+        for (int i = 0; i < numDetections; i++) {
+            int baseIndex = i * valsPerEntry; // Starting index for this detection's data
+            int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex);
+            double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3);
+            double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4);
+            double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5);
+            double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6);
+            double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7);
+            double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8);
+            double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9);
+            double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10);
+            double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11);
+            
+            rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y);
+        }
+    
+        return rawDetections;
+    }
+
+    /**
+     * Prints detailed information about a PoseEstimate to standard output.
+     * Includes timestamp, latency, tag count, tag span, average tag distance,
+     * average tag area, and detailed information about each detected fiducial.
+     *
+     * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
+     */
+    public static void printPoseEstimate(PoseEstimate pose) {
+        if (pose == null) {
+            System.out.println("No PoseEstimate available.");
+            return;
+        }
+    
+        System.out.printf("Pose Estimate Information:%n");
+        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+        System.out.printf("Latency: %.3f ms%n", pose.latency);
+        System.out.printf("Tag Count: %d%n", pose.tagCount);
+        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
+        System.out.println();
+    
+        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+            System.out.println("No RawFiducials data available.");
+            return;
+        }
+    
+        System.out.println("Raw Fiducials Details:");
+        for (int i = 0; i < pose.rawFiducials.length; i++) {
+            RawFiducial fiducial = pose.rawFiducials[i];
+            System.out.printf(" Fiducial #%d:%n", i + 1);
+            System.out.printf("  ID: %d%n", fiducial.id);
+            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
+            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
+            System.out.printf("  TA: %.2f%n", fiducial.ta);
+            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
+            System.out.println();
+        }
+    }
+
+    public static Boolean validPoseEstimate(PoseEstimate pose) {
+        return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+    }
+
+    public static NetworkTable getLimelightNTTable(String tableName) {
+        return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+    }
+
+    public static void Flush() {
+        NetworkTableInstance.getDefault().flush();
+    }
+
+    public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+        return getLimelightNTTable(tableName).getEntry(entryName);
+    }
+
+    public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
+        String key = tableName + "/" + entryName;
+        return doubleArrayEntries.computeIfAbsent(key, k -> {
+            NetworkTable table = getLimelightNTTable(tableName);
+            return table.getDoubleArrayTopic(entryName).getEntry(new double[0]);
+        });
+    }
+    
+    public static double getLimelightNTDouble(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
+    }
+
+    public static void setLimelightNTDouble(String tableName, String entryName, double val) {
+        getLimelightNTTableEntry(tableName, entryName).setDouble(val);
+    }
+
+    public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
+        getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
+    }
+
+    public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
+    }
+
+
+    public static String getLimelightNTString(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getString("");
+    }
+
+    public static String[] getLimelightNTStringArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
+    }
+
+
+    public static URL getLimelightURLString(String tableName, String request) {
+        String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
+        URL url;
+        try {
+            url = new URL(urlString);
+            return url;
+        } catch (MalformedURLException e) {
+            System.err.println("bad LL URL");
+        }
+        return null;
+    }
+    /////
+    /////
+
+    /**
+     * Does the Limelight have a valid target?
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return True if a valid target is present, false otherwise
+     */
+    public static boolean getTV(String limelightName) {
+        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+    }
+
+    /**
+     * Gets the horizontal offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
+    public static double getTX(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tx");
+    }
+
+    /**
+     * Gets the vertical offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
+    public static double getTY(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ty");
+    }
+
+    /**
+     * Gets the horizontal offset from the principal pixel/point to the target in degrees.  This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
+    public static double getTXNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "txnc");
+    }
+
+    /**
+     * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
+    public static double getTYNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tync");
+    }
+
+    /**
+     * Gets the target area as a percentage of the image (0-100%).
+     * @param limelightName Name of the Limelight camera ("" for default) 
+     * @return Target area percentage (0-100)
+     */
+    public static double getTA(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ta");
+    }
+
+    /**
+     * T2D is an array that contains several targeting metrcis
+     * @param limelightName Name of the Limelight camera
+     * @return Array containing  [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, 
+     * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees]
+     */
+    public static double[] getT2DArray(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "t2d");
+    }
+    
+    /**
+     * Gets the number of targets currently detected.
+     * @param limelightName Name of the Limelight camera
+     * @return Number of detected targets
+     */
+    public static int getTargetCount(String limelightName) {
+      double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[1];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the classifier class index from the currently running neural classifier pipeline
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from classifier pipeline
+     */
+    public static int getClassifierClassIndex (String limelightName) {
+    double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[10];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the detector class index from the primary result of the currently running neural detector pipeline.
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from detector pipeline
+     */
+    public static int getDetectorClassIndex (String limelightName) {
+     double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[11];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the current neural classifier result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from classifier pipeline
+     */
+    public static String getClassifierClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tcclass");
+    }
+
+    /**
+     * Gets the primary neural detector result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from detector pipeline
+     */
+    public static String getDetectorClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tdclass");
+    }
+
+    /**
+     * Gets the pipeline's processing latency contribution.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline latency in milliseconds
+     */
+    public static double getLatency_Pipeline(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tl");
+    }
+
+    /**
+     * Gets the capture latency.
+     * @param limelightName Name of the Limelight camera
+     * @return Capture latency in milliseconds
+     */
+    public static double getLatency_Capture(String limelightName) {
+        return getLimelightNTDouble(limelightName, "cl");
+    }
+
+    /**
+     * Gets the active pipeline index.
+     * @param limelightName Name of the Limelight camera
+     * @return Current pipeline index (0-9)
+     */
+    public static double getCurrentPipelineIndex(String limelightName) {
+        return getLimelightNTDouble(limelightName, "getpipe");
+    }
+
+    /**
+     * Gets the current pipeline type.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline type string (e.g. "retro", "apriltag", etc)
+     */
+    public static String getCurrentPipelineType(String limelightName) {
+        return getLimelightNTString(limelightName, "getpipetype");
+    }
+
+    /**
+     * Gets the full JSON results dump.
+     * @param limelightName Name of the Limelight camera
+     * @return JSON string containing all current results
+     */
+    public static String getJSONDump(String limelightName) {
+        return getLimelightNTString(limelightName, "json");
+    }
+
+    /**
+     * Switch to getBotPose
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    /**
+     * Switch to getBotPose_wpiRed
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    /**
+     * Switch to getBotPose_wpiBlue
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    public static double[] getBotPose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    public static double[] getBotPose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+    }
+
+    public static double[] getCameraPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+    }
+
+    public static double[] getTargetPose_CameraSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+    }
+
+    public static double[] getTargetPose_RobotSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+    }
+
+    public static double[] getTargetColor(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "tc");
+    }
+
+    public static double getFiducialID(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tid");
+    }
+
+    public static String getNeuralClassID(String limelightName) {
+        return getLimelightNTString(limelightName, "tclass");
+    }
+
+    public static String[] getRawBarcodeData(String limelightName) {
+        return getLimelightNTStringArray(limelightName, "rawbarcodes");
+    }
+
+    /////
+    /////
+
+    public static Pose3d getBotPose3d(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Red Alliance field space
+     */
+    public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space
+     */
+    public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation relative to the target
+     */
+    public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the target
+     */
+    public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the target's 3D pose with respect to the camera's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the camera
+     */
+    public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the target's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the robot
+     */
+    public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the camera's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the robot
+     */
+    public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
+
+        double[] result = getBotPose_wpiBlue(limelightName);
+        return toPose2D(result);
+    }
+
+    /**
+     * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
+    }
+
+    /**
+     * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+     * Make sure you are calling setRobotOrientation() before calling this method.
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiRed(String limelightName) {
+
+        double[] result = getBotPose_wpiRed(limelightName);
+        return toPose2D(result);
+
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpired", false);
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d(String limelightName) {
+
+        double[] result = getBotPose(limelightName);
+        return toPose2D(result);
+
+    }
+   
+
+
+    /////
+    /////
+
+    public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+        setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+    }
+
+    
+    public static void setPriorityTagID(String limelightName, int ID) {
+        setLimelightNTDouble(limelightName, "priorityid", ID);
+    }
+
+    /**
+     * Sets LED mode to be controlled by the current pipeline.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setLEDMode_PipelineControl(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 0);
+    }
+
+    public static void setLEDMode_ForceOff(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 1);
+    }
+
+    public static void setLEDMode_ForceBlink(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 2);
+    }
+
+    public static void setLEDMode_ForceOn(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 3);
+    }
+
+    /**
+     * Enables standard side-by-side stream mode.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_Standard(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 0);
+    }
+
+    /**
+     * Enables Picture-in-Picture mode with secondary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_PiPMain(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 1);
+    }
+
+    /**
+     * Enables Picture-in-Picture mode with primary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
+    public static void setStreamMode_PiPSecondary(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 2);
+    }
+
+
+    /**
+     * Sets the crop window for the camera. The crop window in the UI must be completely open.
+     * @param limelightName Name of the Limelight camera
+     * @param cropXMin Minimum X value (-1 to 1)
+     * @param cropXMax Maximum X value (-1 to 1)
+     * @param cropYMin Minimum Y value (-1 to 1)
+     * @param cropYMax Maximum Y value (-1 to 1)
+     */
+    public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
+        double[] entries = new double[4];
+        entries[0] = cropXMin;
+        entries[1] = cropXMax;
+        entries[2] = cropYMin;
+        entries[3] = cropYMax;
+        setLimelightNTDoubleArray(limelightName, "crop", entries);
+    }
+   
+    /**
+     * Sets 3D offset point for easy 3D targeting.
+     */
+    public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) {
+        double[] entries = new double[3];
+        entries[0] = offsetX;
+        entries[1] = offsetY;
+        entries[2] = offsetZ;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
+
+    /**
+     * Sets robot orientation values used by MegaTag2 localization algorithm.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
+     * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
+     * @param pitch (Unnecessary) Robot pitch in degrees 
+     * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
+     * @param roll (Unnecessary) Robot roll in degrees
+     * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
+     */
+    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
+    }
+
+    public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
+    }
+
+    private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate, boolean flush) {
+
+        double[] entries = new double[6];
+        entries[0] = yaw;
+        entries[1] = yawRate;
+        entries[2] = pitch;
+        entries[3] = pitchRate;
+        entries[4] = roll;
+        entries[5] = rollRate;
+        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+        if(flush)
+        {
+            Flush();
+        }
+    }
+
+    /**
+     * Sets the 3D point-of-interest offset for the current fiducial pipeline. 
+     * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param x X offset in meters
+     * @param y Y offset in meters
+     * @param z Z offset in meters
+     */
+    public static void SetFidcuial3DOffset(String limelightName, double x, double y, 
+        double z) {
+
+        double[] entries = new double[3];
+        entries[0] = x;
+        entries[1] = y;
+        entries[2] = z;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
+
+    /**
+     * Overrides the valid AprilTag IDs that will be used for localization.
+     * Tags not in this list will be ignored for robot pose estimation.
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param validIDs Array of valid AprilTag IDs to track
+     */
+    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+        double[] validIDsDouble = new double[validIDs.length];
+        for (int i = 0; i < validIDs.length; i++) {
+            validIDsDouble[i] = validIDs[i];
+        }        
+        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+    }
+
+    /**
+     * Sets the downscaling factor for AprilTag detection.
+     * Increasing downscale can improve performance at the cost of potentially reduced detection range.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control.
+     */
+    public static void SetFiducialDownscalingOverride(String limelightName, float downscale) 
+    {
+        int d = 0; // pipeline
+        if (downscale == 1.0)
+        {
+            d = 1;
+        }
+        if (downscale == 1.5)
+        {
+            d = 2;
+        }
+        if (downscale == 2)
+        {
+            d = 3;
+        }
+        if (downscale == 3)
+        {
+            d = 4;
+        }
+        if (downscale == 4)
+        {
+            d = 5;
+        }
+        setLimelightNTDouble(limelightName, "fiducial_downscale_set", d);
+    }
+    
+    /**
+     * Sets the camera pose relative to the robot.
+     * @param limelightName Name of the Limelight camera
+     * @param forward Forward offset in meters
+     * @param side Side offset in meters
+     * @param up Up offset in meters
+     * @param roll Roll angle in degrees
+     * @param pitch Pitch angle in degrees
+     * @param yaw Yaw angle in degrees
+     */
+    public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
+        double[] entries = new double[6];
+        entries[0] = forward;
+        entries[1] = side;
+        entries[2] = up;
+        entries[3] = roll;
+        entries[4] = pitch;
+        entries[5] = yaw;
+        setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
+    }
+
+    /////
+    /////
+
+    public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
+        setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
+    }
+
+    public static double[] getPythonScriptData(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "llpython");
+    }
+
+    /////
+    /////
+
+    /**
+     * Asynchronously take snapshot.
+     */
+    public static CompletableFuture<Boolean> takeSnapshot(String tableName, String snapshotName) {
+        return CompletableFuture.supplyAsync(() -> {
+            return SYNCH_TAKESNAPSHOT(tableName, snapshotName);
+        });
+    }
+
+    private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) {
+        URL url = getLimelightURLString(tableName, "capturesnapshot");
+        try {
+            HttpURLConnection connection = (HttpURLConnection) url.openConnection();
+            connection.setRequestMethod("GET");
+            if (snapshotName != null && snapshotName != "") {
+                connection.setRequestProperty("snapname", snapshotName);
+            }
+
+            int responseCode = connection.getResponseCode();
+            if (responseCode == 200) {
+                return true;
+            } else {
+                System.err.println("Bad LL Request");
+            }
+        } catch (IOException e) {
+            System.err.println(e.getMessage());
+        }
+        return false;
+    }
+
+    /**
+     * Gets the latest JSON results output and returns a LimelightResults object.
+     * @param limelightName Name of the Limelight camera
+     * @return LimelightResults object containing all current target data
+     */
+    public static LimelightResults getLatestResults(String limelightName) {
+
+        long start = System.nanoTime();
+        LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults();
+        if (mapper == null) {
+            mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
+        }
+
+        try {
+            results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
+        } catch (JsonProcessingException e) {
+            results.error = "lljson error: " + e.getMessage();
+        }
+
+        long end = System.nanoTime();
+        double millis = (end - start) * .000001;
+        results.latency_jsonParse = millis;
+        if (profileJSON) {
+            System.out.printf("lljson: %.2f\r\n", millis);
+        }
+
+        return results;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utility/ModeController.java b/src/main/java/frc/robot/utility/ModeController.java
new file mode 100644
index 0000000..072fd42
--- /dev/null
+++ b/src/main/java/frc/robot/utility/ModeController.java
@@ -0,0 +1,206 @@
+package frc.robot.utility;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.IntSupplier;
+
+import edu.wpi.first.wpilibj.XboxController.Axis;
+import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.commands.Intake;
+import frc.robot.commands.ScoreReef;
+
+public class ModeController {
+    IO io;
+    public final CommandGenericHID controller;
+    public final SendableChooser<Runnable> selector = new SendableChooser<Runnable>();
+
+    int mode = 0;
+
+    final int AUTOMATED = 0;
+    final int MANUAL = 1;
+    final int DEMO = 2;
+
+    final int CHARACTERISATION = 3;
+    final int DEBUG = 4;
+
+    enum povDirection {
+        Up,
+        Down,
+
+        Left,
+        LeftUp,
+        LeftDown,
+
+        Right,
+        RightUp,
+        RightDown,
+
+        Centre,
+    }
+
+    final int[] directions = {0, 180, 270, 315, 225, 90, 45, 135, -1};
+    
+
+    public ModeController(IO io, int port){
+        this.io = io;
+        controller = new CommandGenericHID(port);
+        selector.setDefaultOption("Automated", () -> {mode = 0;});
+        selector.addOption("Manual", () -> {mode = 1;});
+        selector.addOption("Characterise", () -> {mode = 2;});
+        selector.addOption("Debug", () -> {mode = 3;});
+        selector.onChange((x) -> {x.run();});
+
+        configureGeneral();
+        configureAutomated();
+        configureManual();
+        configureCharacterisaton();
+        configureDebug();
+    }
+
+    public void toggleMode(){
+        mode = (mode + 1 % 3);
+    }
+
+    public Trigger binding(Button button, int targetMode, BooleanSupplier... conditions){
+        var binding = controller.button(button.value).and(() -> (mode == targetMode));
+        for (int i = 0; i < conditions.length; i++)
+            binding.and(conditions[i]);
+        return binding;
+    }
+
+    public Trigger binding(Axis axis, int targetMode, BooleanSupplier... conditions){
+        var binding = controller.axisMagnitudeGreaterThan(axis.value, 0.2).and(() -> (mode == targetMode));
+        for (int i = 0; i < conditions.length; i++)
+            binding.and(conditions[i]);
+        return binding;
+    }
+
+    public Trigger binding(povDirection direction, int targetMode, BooleanSupplier... conditions){
+        var binding = controller.pov(directions[direction.ordinal()]).and(() -> (mode == targetMode));
+        for (int i = 0; i < conditions.length; i++)
+            binding.and(conditions[i]);
+        return binding;
+    }
+
+    public void configureGeneral(){
+        controller.button(Button.kBack.value).onTrue(Util.Do(io.chassis::resetAngle, io.chassis));
+    }
+
+    public void configureAutomated(){
+        IntSupplier pos = () -> { return ((controller.getHID().getRawButtonPressed(Button.kLeftBumper.value)) ? -1 : 0) + ((controller.getHID().getRawButtonPressed(Button.kRightBumper.value)) ? 1 : 0) + 1; };
+        binding(Button.kY, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 4));
+        binding(Button.kX, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 3));
+        binding(Button.kB, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 2));
+        binding(Button.kA, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 1));
+        binding(Button.kA, AUTOMATED, () -> !io.shooter.coral()).onTrue(new Intake(io, false));
+        binding(Button.kStart, AUTOMATED).onTrue(Util.Do(() -> io.elevator.move(0)));
+
+        // binding(povDirection.Up, AUTOMATED, () -> !io.shooter.algae()).onTrue(new ClearAlgae(io));
+        // binding(povDirection.Up, AUTOMATED, () -> io.shooter.algae()).onTrue(Util.Do(() ->io.shooter.speed(1), io.shooter)).onFalse(Util.Do(() -> {
+        //     io.shooter.stopIntake();
+        //     io.shooter.angle(0);
+        //     io.elevator.move(0);
+        // }, io.shooter));
+
+        // // TODO: Check if this works as expected
+        // binding(povDirection.Left, AUTOMATED, () -> !io.shooter.algae()).toggleOnTrue(Util.Do(() -> {
+        //     io.shooter.speed(-1);
+        //     io.shooter.angle(0);
+        // }, io.shooter)).toggleOnFalse(Util.Do(() -> {
+        //     io.shooter.stopIntake();
+        //     io.shooter.angle(0);
+        // }, io.shooter));
+        // binding(povDirection.Left, AUTOMATED, () -> io.shooter.algae()).onTrue(io.elevator.moveCommand(5));
+
+        binding(povDirection.Down,AUTOMATED).onTrue(Util.Do(io.hang::toggleHang, io.hang));
+    }
+
+    public void configureManual(){
+        binding(Button.kBack, MANUAL).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis));
+        binding(Button.kLeftBumper, MANUAL).onTrue(Util.Do(() ->io.elevator.volts(-4), io.elevator)).onFalse(Util.Do(io.elevator::stop, io.elevator));
+        binding(Button.kRightBumper, MANUAL).onTrue(Util.Do(() ->io.elevator.volts(4), io.elevator)).onFalse(Util.Do(io.elevator::stop, io.elevator));
+
+        // TODO: Check if this is safe
+        binding(Axis.kLeftTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.speed(controller.getRawAxis(Axis.kLeftTrigger.value))));
+        binding(Axis.kRightTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.pivotSpeed(controller.getRawAxis(Axis.kRightTrigger.value))));
+
+        binding(Button.kY, MANUAL).onTrue(Util.Do(() -> io.shooter.speed(-1), io.shooter)).onFalse(Util.Do(io.shooter::stopIntake, io.shooter));
+        binding(Button.kX, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter));
+        binding(Button.kA, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(-.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter));
+
+        binding(povDirection.Down, MANUAL).onTrue(Util.Do(io.chassis::toggle));
+        binding(povDirection.Left, MANUAL).onTrue(Util.Do(io.chassis::syncEncoders));
+        binding(povDirection.Right, MANUAL, () -> !io.chassis.active).onTrue(Util.Do(io.chassis::zeroAbsolute)); // TODO: Add rumble effect
+    }
+
+    boolean mechanismMode = false;
+
+    public void configureCharacterisaton(){
+        binding(Button.kStart, CHARACTERISATION).onTrue(Util.Do(() -> mechanismMode = !mechanismMode));
+
+        binding(Button.kX, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward));
+        binding(Button.kA, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse));
+        binding(Button.kY, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward));
+        binding(Button.kB, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse));
+
+        binding(povDirection.Left, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.quasistatic(Direction.kForward));
+        binding(povDirection.Down, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.quasistatic(Direction.kReverse));
+        binding(povDirection.Right, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kReverse));
+        binding(povDirection.Up, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward));
+        
+        binding(Button.kX, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kForward));
+        binding(Button.kA, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kReverse));
+        binding(Button.kY, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kForward));
+        binding(Button.kB, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kReverse));
+
+        binding(povDirection.Left, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward));
+        binding(povDirection.Down, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse));
+        binding(povDirection.Up, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward));
+        binding(povDirection.Right, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse));
+    }
+
+    public void configureDebug(){
+        for (int i = 0; i < 5; i++){
+            SmartDashboard.putData("Elevator Height L" + i, io.elevator.moveCommand(i));
+        }
+        
+        for (int i = 0; i < 5; i++){
+            SmartDashboard.putData("Shooter Angle L" + i, io.shooter.angleCommand(i));
+        }
+
+        SmartDashboard.putData("Toggle Hang", new InstantCommand(() -> io.hang.toggleHang(), io.hang));
+
+        SmartDashboard.putData("Toggle Elevator Soft Limits", new InstantCommand(() -> io.elevator.toggleSoftLimits(), io.elevator));
+        SmartDashboard.putData("Toggle Shooter Pivot Soft Limits", new InstantCommand(() -> io.shooter.toggleSoftLimits(), io.shooter));
+        
+        SmartDashboard.putData("Toggle Shooter Redundancy", new InstantCommand(() -> io.shooter.togglePivotRedundancy(), io.shooter));
+        SmartDashboard.putData("Toggle Elevator Redundancy", new InstantCommand(() -> io.elevator.toggleRedundancy(), io.elevator));
+        SmartDashboard.putData("Toggle Encoder Redundancy", new InstantCommand(() -> io.chassis.toggleEncoderRedundancy(), io.chassis));
+        SmartDashboard.putData("Toggle Module Redundancy", new InstantCommand(() -> io.chassis.toggleModuleRedundancy(), io.chassis));
+        
+        // TODO: Setup a toggle for Coral Detection Redundancy
+        // TODO: Setup a toggle for Algae Detection Redundancy
+
+        double elevatorkP = (double) Util.get("Elevator kP", 0.3);
+        double elevatorkI = (double) Util.get("Elevator kI", 0.0);
+        double elevatorkD = (double) Util.get("Elevator kD", 0.1);
+        double elevatorkG = (double) Util.get("Elevator kG", 0.0);
+
+        double shooterkP = (double) Util.get("Elevator kP", 0.3);
+        double shooterkI = (double) Util.get("Elevator kI", 0.0);
+        double shooterkD = (double) Util.get("Elevator kD", 0.1);
+        
+        double hangkP = (double) Util.get("Elevator kP", 0.3);
+        double hangkI = (double) Util.get("Elevator kI", 0.0);
+        double hangkD = (double) Util.get("Elevator kD", 0.1);
+
+        SmartDashboard.putData("Set Elevator PID", new InstantCommand(() -> io.elevator.PID(elevatorkP, elevatorkI, elevatorkD, elevatorkG), io.elevator));
+        SmartDashboard.putData("Set Shooter Pivot PID", new InstantCommand(() -> io.shooter.pivotPID(shooterkD, shooterkI, shooterkP), io.shooter));
+        SmartDashboard.putData("Set Hang PID", new InstantCommand(() -> io.hang.PID(hangkP, hangkI, hangkD), io.hang));
+    }
+}
diff --git a/src/main/java/frc/robot/utility/Rumble.java b/src/main/java/frc/robot/utility/Rumble.java
index cc9037e..b0126a4 100644
--- a/src/main/java/frc/robot/utility/Rumble.java
+++ b/src/main/java/frc/robot/utility/Rumble.java
@@ -1,54 +1,38 @@
-// 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.utility;
 
-
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj2.command.*;
-
-public class Rumble extends Command {
-
-Timer time = new Timer();
-double duration = 0;
-boolean condition = false;
-Runnable action;
-GenericHID controller;
-RumblePatterns pattern = new RumblePatterns();
-int type;
- 
-  public Rumble(int type, double duration, GenericHID controller, Runnable action) {
-    time.start();
-    this.type = type;
-    this.controller = controller;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    // pattern.timer.start();
-    action.run();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    pattern.Run(type, controller);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    controller.setRumble(RumbleType.kBothRumble, 0);
-    action.run();
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return time.hasElapsed(duration) || condition;
-  }
+
+public class Rumble {
+
+    public Timer timer = new Timer();
+
+    double duration;
+    GenericHID controller;
+
+    public Rumble(GenericHID controller){
+        this.controller = controller;
+
+    }
+
+    Runnable[] patterns = {
+        () -> { // Basic Shaking
+            controller.setRumble(RumbleType.kBothRumble, .25);
+        }
+    };
+
+    public void Run(double duration, int pattern){
+        this.duration = duration;
+        timer.start();
+        patterns[pattern].run();
+    }
+
+    public boolean finished(){
+        return timer.hasElapsed(duration);
+    }
+
+    public void End(){
+        controller.setRumble(RumbleType.kBothRumble,0);
+    }
 }
diff --git a/src/main/java/frc/robot/utility/RumblePatterns.java b/src/main/java/frc/robot/utility/RumblePatterns.java
deleted file mode 100644
index 4994638..0000000
--- a/src/main/java/frc/robot/utility/RumblePatterns.java
+++ /dev/null
@@ -1,25 +0,0 @@
-package frc.robot.utility;
-
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-
-public class RumblePatterns {
-
-    public Timer timer = new Timer();
-
-    public void Run(int pattern, GenericHID controller){
-        switch(pattern){
-            default:
-                controller.setRumble(RumbleType.kBothRumble, .5);
-            break;
-
-            case 1:
-                if (timer.get()/5.0 % 2.0 == 0.0)
-                    controller.setRumble(RumbleType.kBothRumble, .5);
-                else
-                    controller.setRumble(RumbleType.kBothRumble, 0.0);
-            break;
-        }
-    }
-}
diff --git a/src/main/java/frc/robot/utility/SwerveConstants.java b/src/main/java/frc/robot/utility/SwerveConstants.java
deleted file mode 100644
index fafa34e..0000000
--- a/src/main/java/frc/robot/utility/SwerveConstants.java
+++ /dev/null
@@ -1,20 +0,0 @@
-package frc.robot.utility;
-
-public class SwerveConstants {
-    boolean compChassis = false;
-
-    public static double TRACKWIDTH = 19.5; // 30.0 for MKi
-    public static double WHEELBASE = 21.5;  // 30.0 for MKi
-
-    public static double GEAR_RATIO;
-
-    public static final double MAX_VELOCITY = 5.4;
-
-    public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" };
-
-    public static final int[] CHASSIS_ID = { 2, 3, 4, 5 };  // FL, FR, BL, BR
-    public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR
-
-    public static final int PIGEON_ID = 6;
-
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utility/Util.java b/src/main/java/frc/robot/utility/Util.java
index a869e37..df4e543 100644
--- a/src/main/java/frc/robot/utility/Util.java
+++ b/src/main/java/frc/robot/utility/Util.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.Subsystem;
@@ -40,6 +41,20 @@ public static InstantCommand Do(Runnable action, Subsystem... systems) {
         return new InstantCommand(action, systems);
     }
 
+    public static Command Do(Runnable action, Runnable end, Subsystem... systems) {
+        return Commands.runEnd(action, end, systems);
+    }
+
+    public static InstantCommand blank = new InstantCommand();
+
+    public static Command DoCheck(Command cmd, BooleanSupplier condition, Subsystem... systems){
+        return new ConditionalCommand(cmd, blank, condition);
+    }
+
+    public static Command DoUntil(Runnable action, Runnable end, BooleanSupplier Condition, Subsystem... systems) {
+        return Commands.runEnd(action, end, systems).until(Condition);
+    }
+
     public static ConditionalCommand Do(Command onTrue, Command onFalse, BooleanSupplier condition){
         return new ConditionalCommand(onTrue, onFalse, condition);
     }
diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.2.json
new file mode 100644
index 0000000..a5bf9ee
--- /dev/null
+++ b/vendordeps/PathplannerLib-2025.2.2.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2025.2.2.json",
+    "name": "PathplannerLib",
+    "version": "2025.2.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2025.2.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2025.2.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/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..6af3d3e
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2025.1.1",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.1.1",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2025.1.1",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.1.1",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2025.1.1"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2025.1.1"
+        }
+    ]
+}
\ No newline at end of file