no worky

release the kraken

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\infin\\Downloads",
+    "serverTeam": "2648"
+  }
+}

.OutlineViewer/outlineviewer.json

@@ -0,0 +1,58 @@
+{
+  "Clients": {
+    "open": true
+  },
+  "Connections": {
+    "open": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)"
+  },
+  "client@1": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "client@2": {
+    "open": true
+  },
+  "client@3": {
+    "open": true
+  },
+  "client@4": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "client@5": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@2": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@3": {
+    "open": true
+  },
+  "shuffleboard@1": {
+    "open": true
+  },
+  "transitory": {
+    "Shuffleboard": {
+      "Sensors Tab": {
+        "open": true
+      },
+      "open": true
+    },
+    "orange_Fiducial": {
+      "open": true
+    }
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

WPILib-License.md

@@ -1,24 +1,24 @@
-Copyright (c) 2009-2024 FIRST and other WPILib contributors
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
-All rights reserved.
+All rights reserved.
-
+
-Redistribution and use in source and binary forms, with or without
+Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
+modification, are permitted provided that the following conditions are met:
-   * Redistributions of source code must retain the above copyright
+   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
+     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
+   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in the
+     notice, this list of conditions and the following disclaimer in the
-     documentation and/or other materials provided with the distribution.
+     documentation and/or other materials provided with the distribution.
-   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
-     contributors may be used to endorse or promote products derived from
+     contributors may be used to endorse or promote products derived from
-     this software without specific prior written permission.
+     this software without specific prior written permission.
-
+
-THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

build.gradle

@@ -1,102 +1,112 @@
-plugins {
+plugins {
-    id "java"
+    id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
-}
+}
-
+
-java {
+java {
-    sourceCompatibility = JavaVersion.VERSION_17
+    sourceCompatibility = JavaVersion.VERSION_17
-    targetCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
-}
+}
-
+
-def ROBOT_MAIN_CLASS = "frc.robot.Main"
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
-
+
-// Define my targets (RoboRIO) and artifacts (deployable files)
+task(replayWatch, type: JavaExec) {
-// This is added by GradleRIO's backing project DeployUtils.
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
-deploy {
+    classpath = sourceSets.main.runtimeClasspath
-    targets {
+}
-        roborio(getTargetTypeClass('RoboRIO')) {
+
-            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+// Define my targets (RoboRIO) and artifacts (deployable files)
-            // or from command line. If not found an exception will be thrown.
+// This is added by GradleRIO's backing project DeployUtils.
-            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+deploy {
-            // want to store a team number in this file.
+    targets {
-            team = project.frc.getTeamNumber()
+        roborio(getTargetTypeClass('RoboRIO')) {
-            debug = project.frc.getDebugOrDefault(false)
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
-
+            // or from command line. If not found an exception will be thrown.
-            artifacts {
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
-                // First part is artifact name, 2nd is artifact type
+            // want to store a team number in this file.
-                // getTargetTypeClass is a shortcut to get the class type using a string
+            team = project.frc.getTeamNumber()
-
+            debug = project.frc.getDebugOrDefault(false)
-                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+
-                }
+            artifacts {
-
+                // First part is artifact name, 2nd is artifact type
-                // Static files artifact
+                // getTargetTypeClass is a shortcut to get the class type using a string
-                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+
-                    files = project.fileTree('src/main/deploy')
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
-                    directory = '/home/lvuser/deploy'
+                }
-                }
+
-            }
+                // Static files artifact
-        }
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
-    }
+                    files = project.fileTree('src/main/deploy')
-}
+                    directory = '/home/lvuser/deploy'
-
+                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
-def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+                                           // longer exist in deploy directory of this project
-
+                }
-// Set to true to use debug for JNI.
+            }
-wpi.java.debugJni = false
+        }
-
+    }
-// Set this to true to enable desktop support.
+}
-def includeDesktopSupport = false
+
-
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
-// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+
-// Also defines JUnit 5.
+// Set to true to use debug for JNI.
-dependencies {
+wpi.java.debugJni = false
-    annotationProcessor wpi.java.deps.wpilibAnnotations()
+
-    implementation wpi.java.deps.wpilib()
+// Set this to true to enable desktop support.
-    implementation wpi.java.vendor.java()
+def includeDesktopSupport = false
-
+
-    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+// Also defines JUnit 5.
-
+dependencies {
-    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
-    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+    implementation wpi.java.deps.wpilib()
-
+    implementation wpi.java.vendor.java()
-    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+
-    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
-    simulationDebug wpi.sim.enableDebug()
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
-
+
-    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
-    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
-    simulationRelease wpi.sim.enableRelease()
+
-
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    simulationDebug wpi.sim.enableDebug()
-}
+
-
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
-test {
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
-    useJUnitPlatform()
+    simulationRelease wpi.sim.enableRelease()
-    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+
-}
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-// Simulation configuration (e.g. environment variables).
+
-wpi.sim.addGui().defaultEnabled = true
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
-wpi.sim.addDriverstation()
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
-
+}
-// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+
-// in order to make them all available at runtime. Also adding the manifest so WPILib
+test {
-// knows where to look for our Robot Class.
+    useJUnitPlatform()
-jar {
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
-    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+}
-    from sourceSets.main.allSource
+
-    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+// Simulation configuration (e.g. environment variables).
-    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+wpi.sim.addGui().defaultEnabled = true
-}
+wpi.sim.addDriverstation()
-
+
-// Configure jar and deploy tasks
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
-deployArtifact.jarTask = jar
+// in order to make them all available at runtime. Also adding the manifest so WPILib
-wpi.java.configureExecutableTasks(jar)
+// knows where to look for our Robot Class.
-wpi.java.configureTestTasks(test)
+jar {
-
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
-// Configure string concat to always inline compile
+    from sourceSets.main.allSource
-tasks.withType(JavaCompile) {
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
-    options.compilerArgs.add '-XDstringConcat=inline'
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
-}
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}

gradle/wrapper/gradle-wrapper.properties

@@ -1,7 +1,7 @@
-distributionBase=GRADLE_USER_HOME
+distributionBase=GRADLE_USER_HOME
-distributionPath=permwrapper/dists
+distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
-networkTimeout=10000
+networkTimeout=10000
-validateDistributionUrl=true
+validateDistributionUrl=true
-zipStoreBase=GRADLE_USER_HOME
+zipStoreBase=GRADLE_USER_HOME
-zipStorePath=permwrapper/dists
+zipStorePath=permwrapper/dists

gradlew.bat

@@ -1,94 +1,94 @@
-@rem
+@rem
-@rem Copyright 2015 the original author or authors.
+@rem Copyright 2015 the original author or authors.
-@rem
+@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
+@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 not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
+@rem You may obtain a copy of the License at
-@rem
+@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
+@rem
-@rem Unless required by applicable law or agreed to in writing, software
+@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 distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
+@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
+@rem limitations under the License.
-@rem
+@rem
-@rem SPDX-License-Identifier: Apache-2.0
+@rem SPDX-License-Identifier: Apache-2.0
-@rem
+@rem
-
+
-@if "%DEBUG%"=="" @echo off
+@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
+@rem ##########################################################################
-@rem
+@rem
-@rem  Gradle startup script for Windows
+@rem  Gradle startup script for Windows
-@rem
+@rem
-@rem ##########################################################################
+@rem ##########################################################################
-
+
-@rem Set local scope for the variables with windows NT shell
+@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
+if "%OS%"=="Windows_NT" setlocal
-
+
-set DIRNAME=%~dp0
+set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
+if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
+@rem This is normally unused
-set APP_BASE_NAME=%~n0
+set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
+set APP_HOME=%DIRNAME%
-
+
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+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.
+@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"
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
+
-@rem Find java.exe
+@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
+if defined JAVA_HOME goto findJavaFromJavaHome
-
+
-set JAVA_EXE=java.exe
+set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
+%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
+if %ERRORLEVEL% equ 0 goto execute
-
+
-echo. 1>&2
+echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 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. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 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
+echo location of your Java installation. 1>&2
-
+
-goto fail
+goto fail
-
+
-:findJavaFromJavaHome
+:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
+
-if exist "%JAVA_EXE%" goto execute
+if exist "%JAVA_EXE%" goto execute
-
+
-echo. 1>&2
+echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo. 1>&2
+echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 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
+echo location of your Java installation. 1>&2
-
+
-goto fail
+goto fail
-
+
-:execute
+:execute
-@rem Setup the command line
+@rem Setup the command line
-
+
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
+
-
+
-@rem Execute Gradle
+@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
+
-:end
+:end
-@rem End local scope for the variables with windows NT shell
+@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
+if %ERRORLEVEL% equ 0 goto mainEnd
-
+
-:fail
+:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
+rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
+set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
+exit /b %EXIT_CODE%
-
+
-:mainEnd
+:mainEnd
-if "%OS%"=="Windows_NT" endlocal
+if "%OS%"=="Windows_NT" endlocal
-
+
-:omega
+:omega

settings.gradle

@@ -1,30 +1,30 @@
-import org.gradle.internal.os.OperatingSystem
+import org.gradle.internal.os.OperatingSystem
-
+
-pluginManagement {
+pluginManagement {
-    repositories {
+    repositories {
-        mavenLocal()
+        mavenLocal()
-        gradlePluginPortal()
+        gradlePluginPortal()
-        String frcYear = '2025'
+        String frcYear = '2025'
-        File frcHome
+        File frcHome
-        if (OperatingSystem.current().isWindows()) {
+        if (OperatingSystem.current().isWindows()) {
-            String publicFolder = System.getenv('PUBLIC')
+            String publicFolder = System.getenv('PUBLIC')
-            if (publicFolder == null) {
+            if (publicFolder == null) {
-                publicFolder = "C:\\Users\\Public"
+                publicFolder = "C:\\Users\\Public"
-            }
+            }
-            def homeRoot = new File(publicFolder, "wpilib")
+            def homeRoot = new File(publicFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
+            frcHome = new File(homeRoot, frcYear)
-        } else {
+        } else {
-            def userFolder = System.getProperty("user.home")
+            def userFolder = System.getProperty("user.home")
-            def homeRoot = new File(userFolder, "wpilib")
+            def homeRoot = new File(userFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
+            frcHome = new File(homeRoot, frcYear)
-        }
+        }
-        def frcHomeMaven = new File(frcHome, 'maven')
+        def frcHomeMaven = new File(frcHome, 'maven')
-        maven {
+        maven {
-            name 'frcHome'
+            name = 'frcHome'
-            url frcHomeMaven
+            url = frcHomeMaven
-        }
+        }
-    }
+    }
-}
+}
-
+
-Properties props = System.getProperties();
+Properties props = System.getProperties();
-props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");

src/main/deploy/pathplanner/autos/1 L4 Center.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto

@@ -0,0 +1,74 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Pickup Algae L2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG Algae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Algae"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Post-Barge Backup"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2.5 Coral Left.auto

@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "30 Right to HP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 330 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Backup"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Left.auto

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "30 Right to HP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 330 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Backup"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to K"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Approach"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.257106164383561
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.843032786885245,
+        "y": 6.30266393442623
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": {
+        "x": 2.287270519874242,
+        "y": 6.774371912194027
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -120.06858282186238
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6061643835666786,
+        "y": 5.031720890416444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.065239726031196,
+        "y": 5.647773972598556
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2510679859194649,
+        "y": 7.0812357195448
+      },
+      "prevControl": {
+        "x": 1.6678510274010594,
+        "y": 6.6394691780780075
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.498997995991984,
+      "rotationDegrees": -52.46519085612145
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.16666666666666663,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.98486432191523
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/E to HP.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.958476027397261,
+        "y": 2.837970890410959
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.619606164383562,
+        "y": 1.455607876712329
+      },
+      "isLocked": false,
+      "linkedName": "E"
+    },
+    {
+      "anchor": {
+        "x": 1.1569777397260272,
+        "y": 1.0198630136986298
+      },
+      "prevControl": {
+        "x": 2.7647260273972605,
+        "y": 1.485659246575342
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right HP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.22857142857142831,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "HP Pickup"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 54.162347045721745
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 120.25643716352937
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/H Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.758260140458621,
+        "y": 4.193633481772718
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.112051124065178,
+        "y": 4.206338399805505
+      },
+      "isLocked": false,
+      "linkedName": "H"
+    },
+    {
+      "anchor": {
+        "x": 6.5,
+        "y": 4.021985060730393
+      },
+      "prevControl": {
+        "x": 6.250001988596495,
+        "y": 4.022982203795822
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Behind HG Face"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 0.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HG Algae.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.5,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.224227344787433,
+        "y": 4.009974315068493
+      },
+      "isLocked": false,
+      "linkedName": "Behind HG Face"
+    },
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": {
+        "x": 6.125859753477119,
+        "y": 4.004837296105254
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HG to Barge.path

@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.829105562465259,
+        "y": 3.8676166530198106
+      },
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    },
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": {
+        "x": 7.262336691900294,
+        "y": 4.359747737103425
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.17234468937875755,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Processor Position",
+      "waypointRelativePos": 0.188095238095238,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Processor Position"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 300.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HP to 330 Right.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0019467213114757,
+        "y": 6.434528688524591
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
+      },
+      "prevControl": {
+        "x": 3.2943611350609414,
+        "y": 5.531823506977602
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "L"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.25,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HP to D.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.187029109589041,
+        "y": 0.9747859589041092
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.268878424657535,
+        "y": 1.0949914383561645
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.011857876712329,
+        "y": 2.837970890410959
+      },
+      "prevControl": {
+        "x": 3.3657534246575342,
+        "y": 1.9364297945205475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.27142857142857096,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 58.57043438516136
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 54.24611274556325
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to K.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9266540815180775,
+        "y": 6.744320542331013
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.9679303278688525,
+        "y": 5.211782786885246
+      },
+      "prevControl": {
+        "x": 2.975085616438356,
+        "y": 6.023416095890411
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.49491285058726
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/J Approach.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.286577868852459,
+        "y": 5.955020491803278
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.166700819672132,
+        "y": 5.679303278688525
+      },
+      "isLocked": false,
+      "linkedName": "Before J"
+    },
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
+      },
+      "prevControl": {
+        "x": 5.058811475409836,
+        "y": 5.523463114754099
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/J Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.11855393929902,
+        "y": 5.503390382532524
+      },
+      "isLocked": false,
+      "linkedName": "J"
+    },
+    {
+      "anchor": {
+        "x": 5.442044107776481,
+        "y": 6.005045141603656
+      },
+      "prevControl": {
+        "x": 5.268886874487802,
+        "y": 5.749866060967707
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -118.30075576600632
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/K Approach.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.35655737704918,
+        "y": 5.78719262295082
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.402629510813402,
+        "y": 5.541474576217554
+      },
+      "isLocked": false,
+      "linkedName": "Before K"
+    },
+    {
+      "anchor": {
+        "x": 3.6322745901639335,
+        "y": 5.019979508196721
+      },
+      "prevControl": {
+        "x": 3.596311475409835,
+        "y": 5.271721311475409
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.94539590092286
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/L Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.47740078362179,
+        "y": 5.217924110376135
+      },
+      "isLocked": false,
+      "linkedName": "L"
+    },
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": {
+        "x": 1.3306352459016395,
+        "y": 6.926024590163935
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,54 @@
+{
+  "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": 3.756421232876712,
+        "y": 5.227054794520548
+      },
+      "prevControl": {
+        "x": 2.756421232876712,
+        "y": 5.227054794520548
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.18537788806707
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Post-Barge Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.71311475409836,
+        "y": 4.9480532786885245
+      },
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    },
+    {
+      "anchor": {
+        "x": 5.933913934426228,
+        "y": 5.247745901639345
+      },
+      "prevControl": {
+        "x": 6.641188524590164,
+        "y": 5.043954918032786
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Right Start to E.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.572945205479453,
+        "y": 0.4939640410958907
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.461044520547946,
+        "y": 0.4789383561643841
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.958476027397261,
+        "y": 2.837970890410959
+      },
+      "prevControl": {
+        "x": 5.799914383561644,
+        "y": 1.9664811643835614
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "E"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.4261904761904757,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 120.25643716352937
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.150967037968244,
+        "y": 7.5521014571037055
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.916233431410867,
+        "y": 6.533146539070919
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
+      },
+      "prevControl": {
+        "x": 5.560899253312374,
+        "y": 6.107247922748709
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.25,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Start to H.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.588217213114754,
+        "y": 3.9890368852459024
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.916905737704918,
+        "y": 4.036987704918033
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.758260140458621,
+        "y": 4.193633481772718
+      },
+      "prevControl": {
+        "x": 6.405596206032391,
+        "y": 4.157670367018619
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "H"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.10238095238095252,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,35 @@
+{
+  "robotWidth": 0.8763,
+  "robotLength": 0.8763,
+  "holonomicMode": true,
+  "pathFolders": [
+    "Left Paths",
+    "Center"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.5,
+  "defaultMaxAccel": 1.75,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 400.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 48.35,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.038,
+  "driveGearing": 4.29,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 65.0,
+  "wheelCOF": 1.1,
+  "flModuleX": 0.31115,
+  "flModuleY": 0.31115,
+  "frModuleX": 0.31115,
+  "frModuleY": -0.31115,
+  "blModuleX": -0.31115,
+  "blModuleY": 0.31115,
+  "brModuleX": -0.31115,
+  "brModuleY": -0.31115,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

src/main/java/frc/robot/Robot.java

@@ -4,7 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -14,11 +22,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
+
+
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  @SuppressWarnings("resource")
+public Robot() {
+    Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
+
+    if (isReal()) {
+        Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+    } else {
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+        Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+    }
+
+    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+    }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.

src/main/java/frc/robot/RobotContainer.java

@@ -4,21 +4,35 @@
 
 package frc.robot;
 
+import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.constants.ClimberPivotConstants;
+import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.subsystems.Arm;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.ManipulatorPivot;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
-import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
+
+import java.util.function.IntSupplier;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
-	private Arm arm;
-
 	private ClimberPivot climberPivot;
 
 	private ClimberRollers climberRollers;
@@ -26,17 +40,22 @@ public class RobotContainer {
 	private Drivetrain drivetrain;
 
 	private Elevator elevator;
+	//private ElevatorSysID elevator;
 
-	private Indexer indexer;
+	//private Indexer indexer;
 
 	private Manipulator manipulator;
 
+	private ManipulatorPivot manipulatorPivot;
+
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
-	public RobotContainer() {
+	private SendableChooser<Command> autoChooser;
-		arm = new Arm();
 
+	private IntSupplier closestTag;
+
+	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
@@ -44,79 +63,562 @@ public class RobotContainer {
 		drivetrain = new Drivetrain();
 
 		elevator = new Elevator();
+		//elevator = new ElevatorSysID();
 
-		indexer = new Indexer();
+		//indexer = new Indexer();
 
 		manipulator = new Manipulator();
 
+		manipulatorPivot = new ManipulatorPivot();
+
+		configureNamedCommands();
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
+		autoChooser = AutoBuilder.buildAutoChooser();
+		autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
+		autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
+		autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
+
+		closestTag = drivetrain::getClosestTag;
+
 		configureButtonBindings();
+		//elevatorSysIDBindings();
+		//elevatorOnlyBindings();
+
+		configureShuffleboard();
 	}
 
-	private void configureButtonBindings() {
+	/*private void elevatorSysIDBindings() {
-		arm.setDefaultCommand(
+		elevator.setDefaultCommand(elevator.maintainPosition());
-			arm.goToSetpoint(0, 1)
-		);
 
+		operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
+		operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
+		operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
+		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
+	}*/
+
+	private void configureButtonBindings() {
+		//Default commands
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0, 1)
+			climberPivot.runPivot(() -> 0)
 		);
 
 		climberRollers.setDefaultCommand(
-			climberRollers.runRoller(0)
+			climberRollers.runRoller(() -> 0)
 		);
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				driver::getLeftY,
+				() -> Math.pow(driver.getLeftY(), 3),
-				driver::getLeftX,
+				() -> Math.pow(driver.getLeftX(), 3),
-				driver::getRightX,
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
-
+		
 		elevator.setDefaultCommand(
-			elevator.runElevator(operator::getLeftY)
+			elevator.maintainPosition()
 		);
 
-		indexer.setDefaultCommand(
+		//indexer.setDefaultCommand(
-			indexer.runIndexer(0)
+		//	indexer.runIndexer(() -> 0)
+		//);
+		
+		
+		manipulatorPivot.setDefaultCommand(
+			manipulatorPivot.maintainPosition()
 		);
+		
 
 		manipulator.setDefaultCommand(
-			manipulator.runManipulator(0)
+			manipulator.runManipulator(() -> 0.0, false)
 		);
 
 		//Driver inputs
+		
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(1)
+			manipulator.runManipulator(() -> 0.35, true)
+		);
+
+		driver.leftTrigger().whileTrue( 
+			manipulator.runUntilCollected(() -> 0.75)
+			//.alongWith(indexer.runIndexer(() -> .75))
+			.until(() -> manipulator.getCoralBeamBreak() == false)
+			.andThen(manipulator.retractCommand(() -> .1))
+		);
+
+		driver.start().and(driver.back()).onTrue(
+			startingConfig()
+		);
+
+		driver.y().whileTrue(drivetrain.zeroHeading());
+		
+		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
+
+		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
+
+		driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
+		
+		driver.start().whileTrue(drivetrain.resetToVision());
+	
+		driver.povUp().whileTrue(
+			drivetrain.resetToVision().andThen(
+			drivetrain.goToPose(
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1], 
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+			))
+		);
+
+		driver.rightBumper().whileTrue(
+				drivetrain.resetToVision().andThen(
+				drivetrain.goToPose(
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
+					() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+				))
+			);
+
+		driver.leftBumper().whileTrue(
+			drivetrain.resetToVision().andThen(
+			drivetrain.goToPose(
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+			))
 		);
 
 		//Operator inputs
 		operator.povUp().onTrue(
-			elevator.goToSetpoint(0, 0)
+			safeMoveManipulator(
+				ElevatorConstants.kL4Position, 
+				ManipulatorPivotConstants.kL4Position
+			)
 		);
 
 		operator.povRight().onTrue(
-			elevator.goToSetpoint(0, 0)
+			safeMoveManipulator(
+				ElevatorConstants.kL3Position, 
+				ManipulatorPivotConstants.kL3Position
+			)
 		);
 
 		operator.povLeft().onTrue(
-			elevator.goToSetpoint(0, 0)
+			safeMoveManipulator(
+				ElevatorConstants.kL2Position, 
+				ManipulatorPivotConstants.kL2Position
+			)
 		);
 
 		operator.povDown().onTrue(
-			elevator.goToSetpoint(0, 0)
+			safeMoveManipulator(
+				ElevatorConstants.kL1Position, 
+				ManipulatorPivotConstants.kL1Position
+			)
+		);
+
+		operator.back().onTrue(elevator.homeCommand());
+
+		operator.start().toggleOnTrue(
+			climberPivot.runPivot(() -> -operator.getRightY())
+			.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
+
+		operator.a().onTrue(
+			safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
+		);
+
+		operator.x().onTrue(
+			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
+				.until(() -> driver.a().getAsBoolean())
+		);
+
+		operator.b().onTrue(
+			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
+				.until(() -> driver.a().getAsBoolean())
+		);
+
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
+			.until(() -> driver.a().getAsBoolean())
+		);
+
+		operator.rightTrigger().onTrue(shootAlgae());
+
+	}
+	private void configureNamedCommands() {
+		//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+
+		NamedCommands.registerCommand(
+			"Drivetrain Set X", 
+			drivetrain.setXCommand()
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Coral L4", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 0.4, 
+					true
+				).withTimeout(
+					0.5
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0.0, 
+						false
+					).withTimeout(
+						0.01
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Coral L4 Fast", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 1,
+					true
+				).andThen(
+					manipulator.runManipulator(
+						() -> 1, 
+						true
+					).withTimeout(
+						0.125
+					)
+				).withTimeout(
+					3
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0, 
+						true
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Collect Coral", 
+			manipulator.runUntilCollected(
+				() -> 0.30
+			).andThen(
+				manipulator.runManipulator(
+					() -> 0, 
+					false
+				).withTimeout(
+					0.01
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Lift L4", 
+			safeMoveManipulator(
+				ElevatorConstants.kL4Position, 
+				ManipulatorPivotConstants.kL4Position
+			).andThen(
+				elevator.maintainPosition()
+					.withTimeout(
+						0.1
+					), 
+				manipulatorPivot.maintainPosition()
+					.withTimeout(
+						0.1
+					)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"HP Pickup", 
+			safeMoveManipulator(
+				ElevatorConstants.kCoralIntakePosition, 
+				ManipulatorPivotConstants.kStartingPosition
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Algae",
+			shootAlgae().withTimeout(2)   
+		);
+
+		NamedCommands.registerCommand(
+			"Processor Position", 
+			moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+		);
+
+		NamedCommands.registerCommand(
+			"Pickup Algae L2", 
+			moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+			.andThen(
+				elevator.maintainPosition()
+				.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
+
+			//Dont you need a holdPosition call?
 		);
 	}
 
+	//creates tabs and transforms them on the shuffleboard
+    private void configureShuffleboard() {
+		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
+    	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+		ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
+
+		Shuffleboard.selectTab(OIConstants.kAutoTab);
+
+		autoTab.add("Auto Selection", autoChooser)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kComboBoxChooser);
+
+		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(2, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(2, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+			
+		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
+			.withSize(2, 1)
+			.withPosition(0, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
+			.withSize(1, 1)
+			.withPosition(4, 0)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
+			.withSize(1, 1)
+			.withPosition(4, 1)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
+
+		sensorTab.addDouble("velocity", drivetrain::getVelocity);
+
+		sensorTab.addDouble("heading", drivetrain::getHeading);
+
+
+		//sensorTab.add("odometry", drivetrain::getPose);
+
+		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
+			.withSize(1,1).withPosition(1,1);
+		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
+			.withSize(1,1).withPosition(2,1);
+		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
+			.withSize(1,1).withPosition(3,1);
+		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
+			.withSize(1,1).withPosition(4,1);
+		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
+			.withSize(1,1).withPosition(5,1);
+		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
+			.withSize(1,1).withPosition(6,1);
+
+		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
+			.withSize(1,1).withPosition(1,2);
+		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
+			.withSize(1,1).withPosition(2,2);
+		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
+			.withSize(1,1).withPosition(3,2);
+		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
+			.withSize(1,1).withPosition(4,2);
+		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
+			.withSize(1,1).withPosition(5,2);
+		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
+			.withSize(1,1).withPosition(6,2);
+
+		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
+			.withSize(2,1).withPosition(4,4);
+
+		apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
+			.withSize(2, 1).withPosition(4, 2);
+		apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
+			.withSize(2, 1).withPosition(6, 2);
+
+
+ 	}
+
 	public Command getAutonomousCommand() {
-		return new PrintCommand("NO AUTO DEFINED");
+		return autoChooser.getSelected();
 	}
-}
+
+	/**
+	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
+	 * @return Moves the elevator and arm, then intakes coral
+	 */
+	@SuppressWarnings("unused")
+	private Command coralIntakeRoutine() {
+		return moveManipulator(
+			ElevatorConstants.kCoralIntakePosition, 
+			ManipulatorPivotConstants.kCoralIntakePosition
+		)
+			.andThen(manipulator.runUntilCollected(() -> .5));
+		}
+
+	/**
+	 * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
+	 * 
+	 * @param l2 Is the algae on L2? (True = L2, False = L3)
+	 * @return Moves the elevator and arm then intakes algae
+	 */
+	@SuppressWarnings("unused")
+	private Command algaeIntakeRoutine(boolean l2) {
+		return moveManipulator(
+			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
+			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
+		)
+			.andThen(manipulator.runUntilCollected(() -> 1));
+	}
+
+	/**
+	 * Moves the elevator and arm in different order based on target positions
+	 * 
+	 * @param elevatorPosition The target position of the elevator
+	 * @param armPosition The target rotation of the arm
+	 * @return Moves the elevator and arm to the setpoints using the most efficient path
+	 */
+	private Command moveManipulator(double elevatorPosition, double armPosition) {
+		// If the elevator current and target positions are above the brace, or the arm current and target position is in
+		// front of the brace, move together
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
+			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
+		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
+		// then the elevator, then the arm again
+		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
+		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
+			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
+		// If the arm is behind the brace, move the arm first, then the elevator
+		} else if (!manipulatorPivot.isMotionSafe()) {
+			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
+		// Catch all command that's safe regardless of arm and elevator positions
+		} else { 
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+		}
+	}
+
+	/**
+	 * Moves the elevator and arm in customizeable ways
+	 * 
+	 * @param elevatorPosition The target elevator position
+	 * @param armPosition The target arm position
+	 * @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
+	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
+	 * @return Moves the elevator and arm to the setpoints
+	 */
+	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+		
+		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
+			armPosition = MathUtil.clamp(
+				armPosition, 
+				0,
+				ManipulatorPivotConstants.kRotationLimit
+			);
+		}*/
+		
+		return Commands.either(
+			Commands.either(
+				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				() -> sequential
+			), 
+			Commands.either(
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
+				() -> sequential
+			), 
+			() -> elevatorFirst
+		);
+	}
+
+	@SuppressWarnings("unused")
+	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
+		if(!isL4){
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.goToSetpoint(() -> elevatorPosition),
+				manipulatorPivot.goToSetpoint(() -> armPosition));
+			
+		}else{
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
+		}
+		
+	}
+
+	/**
+	 * Moves the arm and elevator in a safe way.
+	 * 
+	 * @param elevatorPosition The target position of the elevator
+	 * @param armPosition The target rotation of the arm
+	 * @return Moves the elevator and arm to the setpoints
+	 */
+	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
+	private Command moveWithAlgae(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
+	private Command shootAlgae(){
+		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
+	@SuppressWarnings("unused")
+	private Command startingConfig() {
+		return moveManipulatorUtil(0, 0, false, true)
+			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
+	}
+}

src/main/java/frc/robot/constants/ArmConstants.java

@@ -1,10 +0,0 @@
-package frc.robot.constants;
-
-public class ArmConstants {
-    public static final int kArmMotorID = 0;
-    public static final int kCANcoderID = 0;
-
-    public static final double kEncoderConversionFactor = 0;
-
-    public static final double kArmMaxVelocity = 0;
-}

src/main/java/frc/robot/constants/AutoConstants.java

@@ -1,18 +1,50 @@
 package frc.robot.constants;
 
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxSpeedMetersPerSecond = 5;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 1;
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
-    public static final double kPYController = 1;
+
-    public static final double kPThetaController = 1;
+    public static final double kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+        }
+    }
 }

src/main/java/frc/robot/constants/ClimberPivotConstants.java

@@ -1,7 +1,9 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class ClimberPivotConstants {
-    public static final int kPivotMotorID = 0;
+    public static final int kPivotMotorID = 10;
 
     public static final int kClimberLimitSwitchID = 0;
 
@@ -10,4 +12,9 @@ public class ClimberPivotConstants {
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;
     public static final double kPIDControllerD = 0;
+
+    public static final double kClimberClimbPosition = 0;
+    public static final double kClimberStartingPosition = 0;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/ClimberRollersConstants.java

@@ -1,5 +1,18 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
 public class ClimberRollersConstants {
-    public static final int kRollerMotorID = 0;
+    public static final int kRollerMotorID = 9;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        motorConfig
+            .smartCurrentLimit(40)
+            .idleMode(IdleMode.kBrake)
+            .inverted(true);
+        
+    }
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -13,19 +13,26 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.8;
+    public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration
-    public static final double kTrackWidth = Units.inchesToMeters(26.5);
+    public static final double kTrackWidth = Units.inchesToMeters(24.5);
     // Distance between centers of right and left wheels on robot
-    public static final double kWheelBase = Units.inchesToMeters(26.5);
+    public static final double kWheelBase = Units.inchesToMeters(24.5);
 
     // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
+    /* 
-    public static final double kFrontRightChassisAngularOffset = 0;
+    public static final double kFrontLeftChassisAngularOffset = Math.PI;
-    public static final double kBackLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
-    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = 0;
+*/
+
+    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = 0;
 
     // 1, 7, 10 is the default for these three values
     public static final double kSysIDDrivingRampRate = 1;
@@ -38,17 +45,23 @@ public class DrivetrainConstants {
     public static final double kSysIDTurningTimeout = 10;
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 11;
+    public static final int kFrontLeftDrivingCanId = 0;
-    public static final int kRearLeftDrivingCanId = 13;
+    public static final int kRearLeftDrivingCanId = 2;
-    public static final int kFrontRightDrivingCanId = 15;
+    public static final int kFrontRightDrivingCanId = 1;
-    public static final int kRearRightDrivingCanId = 17;
+    public static final int kRearRightDrivingCanId = 3;
 
-    public static final int kFrontLeftTurningCanId = 10;
+    public static final int kFrontLeftTurningCanId = 2;
-    public static final int kRearLeftTurningCanId = 12;
+    public static final int kRearLeftTurningCanId = 4;
-    public static final int kFrontRightTurningCanId = 14;
+    public static final int kFrontRightTurningCanId = 7;
-    public static final int kRearRightTurningCanId = 16;
+    public static final int kRearRightTurningCanId = 5;
+
+    public static final boolean kGyroReversed = true;
+
+    public static final double kHeadingP = 0.1;
+
+    public static final double kXTranslationP = 0.5;
+    public static final double kYTranslationP = 0.5;
 
-    public static final boolean kGyroReversed = false;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
 

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -5,37 +5,71 @@ import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
-    public static final int kElevatorMotor1ID = 0;
+    public static final int kElevatorMotor1ID = 8;
-    public static final int kElevatorMotor2ID = 0;
+    public static final int kElevatorMotor2ID = 6;
 
-    public static final int kTopLimitSwitchID = 0;
     public static final int kBottomLimitSwitchID = 0;
 
-    public static final double kEncoderConversionFactor = 0;
+    // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
+    public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
+    public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
 
-    public static final int kMotorAmpsMax = 0;
+    public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0;
+    /*
-    public static final double kPositionControllerI = 0;
+    public static final double kUpControllerP = 5.6;//7; //
-    public static final double kPositionControllerD = 0;
+    public static final double kUpControllerI = 0;
-    
+    public static final double kUpControllerD = 0.28;//0.28
-    public static final double kVelocityControllerP = 0;
-    public static final double kVelocityControllerI = 0;
-    public static final double kVelocityControllerD = 0;
 
-    public static final double kFeedForwardS = 0;
+    public static final double kDownControllerP = 5.6;//7; //
-    public static final double kFeedForwardG = 0;
+    public static final double kDownControllerI = 0;
-    public static final double kFeedForwardV = 0;
+    public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
+
+    public static final double kMaintainP = 3;
+    */
+
+    public static final double kP = 3;//7; //
+    public static final double kI = 0;
+    public static final double kD = 0;//.28;//0.28 
+
+    public static final double kAllowedError = 1;
+
+    public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8;   /* kG too high - kG too low / 2  0.95, 0.2 */
+    public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
+    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
+
+    public static final double kMaxVelocity = 100.0;    // 100 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxAcceleration = 50;    // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocityAlgae = 120;
+    public static final double kMaxAccelerationAlgae = 400;
+
+    public static final double kCoralIntakePosition = 0;
+    public static final double kL1Position = 17;
+    public static final double kL2Position = 11;
+    public static final double kL3Position = 27;
+    public static final double kL4Position = 50.5;
+    public static final double kL4TransitionPosition = 40.0;
+    public static final double kL2AlgaePosition = 23.0;
+    public static final double kL3AlgaePosition = 39.0;
+    public static final double kProcessorPosition = 4.0;
+    /**The position of the top of the elevator brace */
+    public static final double kBracePosition = 0;
+    public static final double kMaxHeight = 51.0; //actual is 51
+
+    public static final double kVoltageLimit = 7;
+
+    public static final double kVoltageLimitAlgae = 9;
 
     // 1, 7, 10 are the defaults for these, change as necessary
-    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDRampRate = .25;
-    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDStepVolts = 3;
     public static final double kSysIDTimeout = 10;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -52,10 +86,15 @@ public class ElevatorConstants {
     
     static {
         motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
+            .smartCurrentLimit(kCurrentLimit)
-            .idleMode(kIdleMode);
+            .idleMode(kIdleMode)
+            .inverted(true);
         motorConfig.encoder
-            .positionConversionFactor(kEncoderConversionFactor)
+            .positionConversionFactor(kEncoderPositionConversionFactor)
-            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
+            .velocityConversionFactor(kEncoderVelocityConversionFactor);
+        motorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1);
     }
 }

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -1,6 +1,15 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class IndexerConstants {
-    public static final int kIndexerMotorID = 0;
+    public static final int kIndexerMotorID = 16;
-    public static final int kIndexerBeamBreakID = 0;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(30)
+        .idleMode(IdleMode.kBrake);
+    };
 }

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -1,7 +1,16 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class ManipulatorConstants {
-    public static final int kManipulatorMotorID = 0;
+    public static final int kManipulatorMotorID = 12;
-    public static final int kCoralBeamBreakID = 0;
+    public static final int kCoralBeamBreakID = 2;
-    public static final int kAlgaeBeamBreakID = 0;
+    
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(40)
+        .idleMode(IdleMode.kBrake);
+    };
 }

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -0,0 +1,86 @@
+package frc.robot.constants;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+
+public class ManipulatorPivotConstants {
+    public static final int kPivotMotorID = 1;
+
+    public static final int kMotorCurrentMax = 40;
+
+    public static final double kPivotConversion = 2 * Math.PI;
+
+    public static final double kPivotMaxVelocity = 2 * Math.PI;
+
+    public static final double kPositionalP = 4.5;
+    public static final double kPositionalI = 0;
+    public static final double kPositionalD = 0;
+    public static final double kPositionalTolerance = Units.degreesToRadians(3);
+
+    public static final double kAlgaeP = 7;
+
+    public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
+    public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
+    public static final double kFeedForwardV = 0.68; //calculated value 0.68
+
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
+
+    public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
+    public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
+
+    public static final double kEncoderOffset = 0.04500000178813934;
+
+    public static final double kStartingPosition = Units.degreesToRadians(90);
+    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL1Position = Units.degreesToRadians(246);
+    public static final double kL2Position = Units.degreesToRadians(22.0+90);
+    public static final double kL3Position = Units.degreesToRadians(22.0+90);
+    public static final double kL4Position = Units.degreesToRadians(45.0+90);
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
+    public static final double kNetPosition = Units.degreesToRadians(175.0+90);
+    /**The closest position to the elevator brace without hitting it */
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
+
+    public static final double kBargeShotPosition = Units.degreesToRadians(222);
+    /**The forward rotation limit of the pivot */
+    public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
+
+    public static final SysIdRoutine.Config kSysIDConfig = new Config(
+        Volts.of(kSysIDRampRate).per(Second),
+        Volts.of(kSysIDStepVolts),
+        Seconds.of(kSysIDTimeout)
+    );
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        motorConfig
+            .smartCurrentLimit(kMotorCurrentMax)
+            .idleMode(kIdleMode)
+            .inverted(true);
+        motorConfig.absoluteEncoder
+            .positionConversionFactor(kPivotConversion)
+            .inverted(false)
+            .zeroOffset(kEncoderOffset);
+    }
+
+
+}

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -2,6 +2,13 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ModuleConstants {
@@ -16,56 +23,83 @@ public class ModuleConstants {
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
     // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
     // teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
-    public static final int kDriveMotorCurrentLimit = 40;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kTurningFactor = 2 * Math.PI;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    
+    public static final double kDriveP = .04;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+    
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final int kDriveMotorStatorCurrentLimit = 100;
+    public static final int kDriveMotorSupplyCurrentLimit = 65;
     public static final int kTurnMotorCurrentLimit = 20;
 
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM 
 
-    public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
     public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
 
-    static {
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
-        // Use module constants to calculate conversion factors and feed forward gain.
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
-        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
-        double turningFactor = 2 * Math.PI;
+    public static final AudioConfigs kAudioConfig = new AudioConfigs();
-        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
 
-        drivingConfig
+    static {
-            .idleMode(IdleMode.kBrake)
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
-            .smartCurrentLimit(kDriveMotorCurrentLimit);
+
-        drivingConfig.encoder
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
-            .positionConversionFactor(drivingFactor) // meters
+        kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
-            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
-        drivingConfig.closedLoop
+        kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+
-            // These are example gains you may need to them for your own robot!
+        kDriveMotorConfig.Inverted = kDriveInversionState;
-            .pid(0.04, 0, 0)
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
-            .velocityFF(drivingVelocityFeedForward)
+
-            .outputRange(-1, 1);
+        kAudioConfig.AllowMusicDurDisable = true;
+
+        kDriveSlot0Config.kP = kDriveP;
+        kDriveSlot0Config.kI = kDriveI;
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
 
         turningConfig
-            .idleMode(IdleMode.kBrake)
+            .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(20);
+            .smartCurrentLimit(kTurnMotorCurrentLimit);
         turningConfig.absoluteEncoder
             // Invert the turning encoder, since the output shaft rotates in the opposite
             // direction of the steering motor in the MAXSwerve Module.
             .inverted(true)
-            .positionConversionFactor(turningFactor) // radians
+            .positionConversionFactor(kTurningFactor) // radians
-            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+            .velocityConversionFactor(kTurningFactor / 60.0); // radians per second
         turningConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             // These are example gains you may need to them for your own robot!
-            .pid(1, 0, 0)
+            .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
             // Enable PID wrap around for the turning motor. This will allow the PID
             // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
             // to 10 degrees will go through 0 rather than the other direction which is a
             // longer route.
             .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, turningFactor);
+            .positionWrappingInputRange(0, kTurningFactor);
     }
 }

src/main/java/frc/robot/constants/NeoMotorConstants.java

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class NeoMotorConstants {
-    public static final double kFreeSpeedRpm = 5676;
+    public static final double kFreeSpeedRpm = 6000; //for kraken not neo
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -4,5 +4,9 @@ public class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kOperatorControllerPort = 1;
     
-    public static final double kDriveDeadband = 0.05;
+    public static final double kDriveDeadband = Math.pow(0.05, 3);
+
+    public static final String kAutoTab = "Auto Tab";
+    public static final String kSensorsTab = "Sensors Tab";
+    public static final String kApriltagTab = "Apriltag Tab";
 }

src/main/java/frc/robot/constants/VisionConstants.java

@@ -0,0 +1,90 @@
+package frc.robot.constants;
+
+public class VisionConstants {
+    
+    // global coordinate map of all tags. index is the tag id.
+    // Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
+    // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
+    public static final double[][] globalTagCoords = {{},
+        {656.98, 24.73, 58.50, 126.0, 0},
+        {656.98, 291.90, 58.50, 234.0, 0},
+        {452.4, 316.21, 51.25, 270, 0},
+        {365.2, 241.44, 73.54, 0, 30},
+        {365.2, 75.19, 73.54, 0, 30},
+        {530.49, 129.97, 12.13, 300, 0},
+        {546.87, 158.3, 12.13, 0, 0},
+        {530.49, 186.63, 12.13, 60, 0},
+        {497.77, 186.63, 12.13, 120, 0},
+        {481.39, 158.3, 12.13, 180, 0},
+        {497.77, 129.97, 12.13, 240, 0},
+        {33.9, 24.73, 58.5, 54, 0},
+        {33.9, 291.9, 58.5, 306, 0},
+        {325.68, 241.44, 73.54, 180, 30},
+        {325.68, 75.19, 73.54, 180, 30},
+        {238.49, 0.42, 51.25, 90, 0},
+        {160.39, 129.97, 12.13, 240, 0},
+        {144.00, 158.3, 12.13, 180, 0},
+        {160.39, 186.63, 12.13, 120, 0},
+        {193.1, 186.63, 12.13, 60, 0},
+        {209.49, 158.3, 12.13, 0, 0},
+        {193.1, 129.97, 12.13, 300, 0},
+    };
+
+    //map of coral placing setpoints based on the tag that is on the same reef face
+    // and the on the left or right branch of that side of the reef
+    // <tag_number, {left_x, left_y, right_x, right_y}>
+    public static final double[][] reefSetpointsMap = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {13.570, 2.816, 13.858, 2.970},//6
+        {14.373, 3.862, 14.385, 4.194},
+        {13.858, 5.032, 13.558, 5.227},
+        {12.575, 5.227, 12.287, 5.056},
+        {11.772, 4.169, 11.772, 3.845},
+        {12.287, 2.982, 12.587, 2.826},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.703, 3.975, 3.982, 2.806},
+        {3.183, 4.191, 3.183, 3.857},
+        {3.986, 5.24, 3.701, 5.076},
+        {5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
+        {5.789, 3.862, 5.789, 4.194},
+        {4.993, 2.816, 5.272, 2.996}
+    };
+
+    public static final double[][] algaeSetpointsMap = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {13.906, 2.658},//6
+        {14.661, 4.013},
+        {13.834, 5.428},
+        {12.263, 5.452},
+        {11.412, 4.025},
+        {12.191, 2.574},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.649, 2.558},//17
+        {2.776, 4.005},
+        {3.644, 5.514},
+        {5.296, 5.522},//4.991, 5.246},
+        {6.225, 4.008},
+        {5.322, 2.511}//22
+    };
+
+    public static final double latencyFudge = 0.0;
+
+}

src/main/java/frc/robot/subsystems/Arm.java

@@ -1,68 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ArmConstants;
-
-public class Arm extends SubsystemBase {
-    private SparkMax armMotor;
-
-    private CANcoder canCoder;
-
-    private PIDController positionController;
-    private PIDController velocityController;
-
-    private ArmFeedforward feedForward;
-
-    public Arm() {
-        armMotor = new SparkMax(
-            ArmConstants.kArmMotorID, 
-            MotorType.kBrushless
-        );
-
-        canCoder = new CANcoder(ArmConstants.kCANcoderID);
-    }
-
-    //manual command that keeps ouput speed consistent no matter the direction
-    public Command runArm(DoubleSupplier speed) {
-        return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
-
-            double voltsOut = velocityController.calculate(
-                rotationsToRadians(canCoder.getVelocity().getValueAsDouble()), 
-                realSpeedTarget
-            ) + feedForward.calculate(
-                rotationsToRadians(canCoder.getPosition().getValueAsDouble()), 
-                canCoder.getVelocity().getValueAsDouble()
-            );
-
-            armMotor.setVoltage(voltsOut);
-        });
-    }
-
-    public Command goToSetpoint(double setpoint, double timeout) {
-        return run(() -> {
-            double voltsOut = positionController.calculate(
-                canCoder.getPosition().getValueAsDouble(), 
-                setpoint
-            ) + feedForward.calculate(
-                canCoder.getPosition().getValueAsDouble(), 
-                canCoder.getVelocity().getValueAsDouble()
-            );
-
-            armMotor.setVoltage(voltsOut);
-        }).until(positionController::atSetpoint).withTimeout(timeout);
-    }
-
-    protected double rotationsToRadians(double rotations) {
-        return rotations * 2 * Math.PI;
-    }
-}

src/main/java/frc/robot/subsystems/ClimberPivot.java

@@ -1,11 +1,13 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberPivotConstants;
@@ -15,45 +17,41 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
-    private DigitalInput cageLimitSwitch;
-
-    private PIDController pidController;
-
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
 
-        neoEncoder = pivotMotor.getEncoder();
+        pivotMotor.configure(
-
+            ClimberPivotConstants.motorConfig, 
-        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
+            ResetMode.kResetSafeParameters, 
-
+            PersistMode.kPersistParameters
-        pidController = new PIDController(
-            ClimberPivotConstants.kPIDControllerP,
-            ClimberPivotConstants.kPIDControllerI, 
-            ClimberPivotConstants.kPIDControllerD
         );
+
+        neoEncoder = pivotMotor.getEncoder();
     }
 
-    public Command runPivot(double speed) {
+    public Command runPivot(DoubleSupplier speed) {
         return run(() -> {
-            pivotMotor.set(speed);
+            pivotMotor.set(speed.getAsDouble());
         });
     }
 
-    public Command goToAngle(double setpoint, double timeout) {
+    /**
+     * Runs the climber until it is at setpoint
+     * 
+     * @param speed The speed at which the pivot runs
+     * @param setpoint The target position of the climber
+     * @return Sets the motor speed until at the target position
+     */
+    public Command climb(double setpoint, double speed) {
         return run(() -> {
-            pivotMotor.set(
+            pivotMotor.set(speed);
-                pidController.calculate(
+        }).until(() -> neoEncoder.getPosition() >= setpoint);
-                    neoEncoder.getPosition(), 
-                    setpoint
-                )
-            );
-        }).withTimeout(timeout);
     }
 
-    public boolean getCageLimitSwitch() {
+    public double getEncoderPosition() {
-        return cageLimitSwitch.get();
+        return neoEncoder.getPosition();
     }
-}
+}

src/main/java/frc/robot/subsystems/ClimberRollers.java

@@ -1,12 +1,17 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberRollersConstants;
 
+//TODO Figure out a way to detect if we're at the top of the cage
 public class ClimberRollers extends SubsystemBase {
     private SparkMax rollerMotor;
 
@@ -15,11 +20,23 @@ public class ClimberRollers extends SubsystemBase {
             ClimberRollersConstants.kRollerMotorID, 
             MotorType.kBrushless
         );
+
+        rollerMotor.configure(
+            ClimberRollersConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
     }
 
-    public Command runRoller(double speed) {
+    /**
+     * Runs the rollers at a set speed
+     * 
+     * @param speed The speed in which the roller runs
+     * @return Runs the rollers at a set speed
+     */
+    public Command runRoller(DoubleSupplier speed) {
         return run(() -> {
-            rollerMotor.set(speed);
+            rollerMotor.set(speed.getAsDouble());
         });
     }
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -4,23 +4,45 @@
 
 package frc.robot.subsystems;
 
+import java.io.File;
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.Orchestra;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+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.interpolation.TimeInterpolatableBuffer;
 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.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.VisionConstants;
+import edu.wpi.first.math.geometry.Pose3d;
 
 public class Drivetrain extends SubsystemBase {
     // Create MAXSwerveModules
@@ -30,10 +52,26 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private ADIS16470_IMU m_gyro;
+    private AHRS gyro;
 
     // Odometry class for tracking robot pose
-    private SwerveDriveOdometry m_odometry;
+    private SwerveDrivePoseEstimator m_estimator;
+    
+    private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
+
+    public Orchestra m_orchestra = new Orchestra();
+    private Timer musicTimer = new Timer();
+
+    private ProfiledPIDController pidHeading;
+    private PIDController pidTranslationX;
+    private PIDController pidTranslationY;
+
+    private HolonomicDriveController driveController;
+
+    public Vision vision;
+
+    public Pose2d orangePose2d;
+    public Pose2d blackPose2d;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -61,30 +99,169 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        m_gyro = new ADIS16470_IMU();
+        gyro = new AHRS(NavXComType.kMXP_SPI);
 
-        m_odometry = new SwerveDriveOdometry(
+        m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
-        });
+            },
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
+        );
+
+        pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
+        pidHeading.setTolerance(Units.degreesToRadians(3));
+        pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
+
+        pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
+        pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
+        pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
+        pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
+
+        driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
+
+        AutoBuilder.configure(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPPDriveController, 
+            AutoConstants.kRobotConfig,
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
+
+        m_orchestra.loadMusic(Filesystem.getDeployDirectory()
+        .toPath()
+        .resolve("Orchestra" + File.separator + "doomE1M1.chrp")
+        .toString());
+
+        // Add a single device to the orchestra
+        m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
+        m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
+        m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
+        m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
+
+        m_orchestra.play();
+        musicTimer.reset();
+        musicTimer.start();
+
+        vision = new Vision();
+        orangePose2d = new Pose2d();
+        blackPose2d = new Pose2d();
     }
 
     @Override
     public void periodic() {
         // Update the odometry in the periodic block
-        m_odometry.update(
+        m_estimator.update(
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
+
+        gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
+
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
+
+        if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
+            if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+            }
+
+            // if the detected tags match your alliances reef tags use their pose estimates  
+            if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
+                orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
+
+            }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
+                orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
+            } 
+        }
+        Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
+        Logger.recordOutput("orange dist", vision.getOrangeDist());
+        Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange FPS", vision.getOrangeFPS());
+
+
+        if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
+            if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+            }
+
+            if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
+
+            }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
+            }
+        }
+        Logger.recordOutput("black pose", new Pose3d(blackPose2d));
+        Logger.recordOutput("black dist", vision.getBlackDist());
+        Logger.recordOutput("black detected", vision.getBlackTagDetected());
+        Logger.recordOutput("black tag", vision.getBlackTagDetected());
+        Logger.recordOutput("black FPS", vision.getBlackFPS());
+
+        Logger.recordOutput("drive velocity", getVelocity());
+        Logger.recordOutput("closest tag", getClosestTag());
+        Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
+        
+        if(musicTimer.get()>10){
+            if (m_orchestra.isPlaying()) {
+                m_orchestra.stop();
+            }
+            musicTimer.stop();
+            musicTimer.reset();
+        }
+        
+    }
+
+    public ChassisSpeeds getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
+            m_frontLeft.getState(),
+            m_frontRight.getState(),
+            m_rearLeft.getState(),
+            m_rearRight.getState()
+        );
+    }
+
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+
+        setModuleStates(newStates);
     }
 
     /**
@@ -93,7 +270,7 @@ public class Drivetrain extends SubsystemBase {
      * @return The pose.
      */
     public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_estimator.getEstimatedPosition();
     }
 
     /**
@@ -102,8 +279,9 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_odometry.resetPosition(
+
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+        m_estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -117,12 +295,20 @@ public class Drivetrain extends SubsystemBase {
     public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
             BooleanSupplier fieldRelative) {
         return run(() -> {
-            drive(
+            if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
-                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                drive(
-                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                fieldRelative.getAsBoolean()
+                    -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
-            );
+                    fieldRelative.getAsBoolean()
+                );
+            }else{
+                drive(
+                    MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
+                    fieldRelative.getAsBoolean()
+            );}
         });
     }
 
@@ -144,7 +330,7 @@ public class Drivetrain extends SubsystemBase {
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
+                        new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -160,6 +346,71 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
+        return startRun(() -> {
+                pidTranslationX.reset();
+                pidTranslationY.reset();
+                pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
+            },
+            () -> {
+                    ChassisSpeeds controlEffort = driveController.calculate(
+                    m_estimator.getEstimatedPosition(),
+                    new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
+                    headingSetpoint.get()),
+                    0,
+                    headingSetpoint.get()
+                );
+
+                double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
+                    controlEffort.vxMetersPerSecond *= mul;
+                    controlEffort.vyMetersPerSecond *= mul;
+                }
+
+                driveWithChassisSpeeds(controlEffort);
+
+                Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
+                    new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
+                    headingSetpoint.get())));
+            });
+    }
+
+    public int getClosestTag(){
+
+		if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
+			int closestTag = 17;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
+			+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
+			
+			for(int i = 17; i <= 22; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
+				+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}else{
+			int closestTag = 6;
+			double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
+			+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
+			
+			for(int i = 6; i <= 11; ++i){
+				double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
+				+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}
+	}
+
     /**
      * Sets the wheels into an X formation to prevent movement.
      */
@@ -192,9 +443,17 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.resetEncoders();
     }
 
-    /** Zeroes the heading of the robot. */
+    /** Zeroes the heading of the robot. 
-    public void zeroHeading() {
+     * @return */
-        m_gyro.reset();
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+            m_estimator.resetRotation(new Rotation2d(0));
+        });
+    }
+
+    public double getGyroValue() {
+        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -203,7 +462,11 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
+        return m_estimator.getEstimatedPosition().getRotation().getDegrees();
+    }
+
+    public TimeInterpolatableBuffer<Double> getGyroBuffer(){
+        return gyroBuffer;
     }
 
     /**
@@ -212,6 +475,20 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+    }
+
+    public double getVelocity(){
+        return m_frontLeft.getState().speedMetersPerSecond;
+    }
+
+    public Command resetToVision(){
+        return runOnce(() -> {
+            if(vision.getOrangeTagDetected()){
+                m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }else if(vision.getBlackTagDetected()){
+                m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }
+        });
     }
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -2,16 +2,20 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -23,13 +27,16 @@ public class Elevator extends SubsystemBase {
 
     protected RelativeEncoder encoder;
 
-    private DigitalInput topLimitSwitch;
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController positionController;
-    private PIDController velocityController;
-
     private ElevatorFeedforward feedForward;
+
+    private TrapezoidProfile trapProfile;
+    private TrapezoidProfile trapProfileAlgae;
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State setpoint;
+
+    private SparkClosedLoopController controller;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -56,59 +63,181 @@ public class Elevator extends SubsystemBase {
 
         encoder = elevatorMotor1.getEncoder();
 
-        topLimitSwitch = new DigitalInput(
-            ElevatorConstants.kTopLimitSwitchID
-        );
-
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        positionController = new PIDController(
-            ElevatorConstants.kPositionControllerP, 
-            ElevatorConstants.kPositionControllerI, 
-            ElevatorConstants.kPositionControllerD
-        );
-
-        velocityController = new PIDController(
-            ElevatorConstants.kVelocityControllerP, 
-            ElevatorConstants.kVelocityControllerI, 
-            ElevatorConstants.kVelocityControllerD
-        );
-
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
+
+        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
+
+        trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
+
+        controller = elevatorMotor1.getClosedLoopController();
     }
 
-    //manual command that keeps ouput speed consistent no matter the direction
+    @Override
-    public Command runElevator(DoubleSupplier speed) {
+    public void periodic() {
-        return run(() -> {
+        if (!getBottomLimitSwitch()) {
-            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
+            encoder.setPosition(0);
+        }
 
-            double voltsOut = velocityController.calculate(
+        Logger.recordOutput("elevator position", getEncoderPosition());
-                encoder.getVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(realSpeedTarget);
-
-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get());
     }
 
+    /**
+     * Returns whether or not the motion is safe relative to the encoder's current position
+     * and the elevator brace position
+     * 
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe() {
+        return isMotionSafe(getEncoderPosition());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to some target position and the elevator
+     * brace position
+     * 
+     * @param motionTarget The target position to determine the safety of
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe(double motionTarget) {
+        return motionTarget > ElevatorConstants.kBracePosition;
+    }
     
-    //go to setpoint command
+    /**
-    public Command goToSetpoint(double setpoint, double timeout) {
+     * A manual translation command that uses feed forward calculation to maintain position
+     * 
+     * @param speed The speed at which the elevator translates
+     * @return Sets motor voltage to translate the elevator and maintain position
+     */
+    public Command runManualElevator(DoubleSupplier speed) {
         return run(() -> {
-            double voltsOut = positionController.calculate(
+            double desired = speed.getAsDouble();
-                encoder.getPosition(),
-                setpoint
-            ) + feedForward.calculate(0);
 
-            elevatorMotor1.setVoltage(voltsOut);
+            if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
-        }).until(
+                elevatorMotor1.set(
-            () -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
+                    speed.getAsDouble()
-        ).withTimeout(timeout);
+                );
+            } else {
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
+            }
+            
+        });
     }
-}
+
+    /**
+     * A command that will use the feed forward to hold up the elevator.
+     * Used for feed forward tuning.
+     * 
+     * @return Sets motor voltage based on feed forward calculation.
+     */
+    public Command maintainPosition() {
+
+        return startRun(() -> {
+
+        }, 
+        () -> {
+            controller.setReference(
+                encoder.getPosition(), 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(0.0)
+            );
+
+        });
+        
+    }
+
+    public Command homeCommand(){
+        return run(() -> {
+            elevatorMotor1.setVoltage(0.5);
+        })
+        .until(() -> elevatorMotor1.getOutputCurrent() > 5)
+        .andThen(run(() -> encoder.setPosition(0)));
+    }
+
+    /**
+     * Moves the elevator to a target destination (setpoint). 
+     * 
+     * @param setpoint Target destination of the subsystem
+     * @return Sets motor voltage to achieve the target destination
+     */
+    public Command goToSetpoint(DoubleSupplier setGoal) {
+        
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(encoder.getVelocity())
+            );
+
+        }).until(() -> trapProfile.isFinished(encoder.getPosition()));
+
+    }          
+
+    public Command goToSetpointAlgae(DoubleSupplier setGoal) {
+            
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(setpoint.velocity)
+            );
+
+        }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
+    }   
+
+    /**
+     * Returns the current encoder position
+     * 
+     * @return Current encoder position
+     */
+    public double getEncoderPosition() {
+        return encoder.getPosition();
+    }
+
+
+    /**
+     * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
+     * 
+     * @return The value of bottomLimitSwitch
+     */
+    public boolean getBottomLimitSwitch() {
+        return bottomLimitSwitch.get();
+    }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor1() {
+        return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
+    }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor2() {
+        return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
+    }
+
+}

src/main/java/frc/robot/subsystems/Indexer.java

@@ -1,36 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IndexerConstants;
-
-public class Indexer extends SubsystemBase {
-    private SparkMax indexerMotor;
-
-    private DigitalInput indexerBeamBreak;
-
-    public Indexer() {
-        indexerMotor = new SparkMax(
-            IndexerConstants.kIndexerMotorID, 
-            MotorType.kBrushless
-        );
-
-        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID);
-    }
-
-    public Command runIndexer(double speed) {
-        return run(() -> {
-            indexerMotor.set(speed);
-        });
-    }
-
-    public Command indexCoral(double speed) {
-        return run(() -> {
-            indexerMotor.set(speed);
-        }).until(indexerBeamBreak::get);
-    }
-}

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -15,117 +15,133 @@ 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.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.RelativeEncoder;
 
 import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
-  private final SparkMax m_drivingSpark;
+    private final TalonFX m_drive;
-  private final SparkMax m_turningSpark;
+    private final SparkMax m_turningSpark;
 
-  private final RelativeEncoder m_drivingEncoder;
+    private final AbsoluteEncoder m_turningEncoder;
-  private final AbsoluteEncoder m_turningEncoder;
 
-  private final SparkClosedLoopController m_drivingClosedLoopController;
+    private final SparkClosedLoopController m_turningClosedLoopController;
-  private final SparkClosedLoopController m_turningClosedLoopController;
 
-  private double m_chassisAngularOffset = 0;
+    private final VelocityVoltage driveVelocityRequest;
-  private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
-  /**
+    private double m_chassisAngularOffset = 0;
-   * Constructs a MAXSwerveModule and configures the driving and turning motor,
+    private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
-   * encoder, and PID controller. This configuration is specific to the REV
-   * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
-   * Encoder.
-   */
-  public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
-    m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_drivingEncoder = m_drivingSpark.getEncoder();
+    /**
-    m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
+     * Constructs a MAXSwerveModule and configures the driving and turning motor,
+     * encoder, and PID controller. This configuration is specific to the REV
+     * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
+     * Encoder.
+     */
+    public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
+        m_drive = new TalonFX(drivingCANId);
+        m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
+        m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
-    m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
 
-    // Apply the respective configurations to the SPARKS. Reset parameters before
+        m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
-    // applying the configuration to bring the SPARK to a known good state. Persist
-    // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
-        PersistMode.kPersistParameters);
-    m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
-        PersistMode.kPersistParameters);
 
-    m_chassisAngularOffset = chassisAngularOffset;
+        driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
-    m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drivingEncoder.setPosition(0);
-  }
 
-  /**
+        // Apply the respective configurations to the SPARKS. Reset parameters before
-   * Returns the current state of the module.
+        // applying the configuration to bring the SPARK to a known good state. Persist
-   *
+        // the settings to the SPARK to avoid losing them on a power cycle.
-   * @return The current state of the module.
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-   */
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
-  public SwerveModuleState getState() {
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
-    // Apply chassis angular offset to the encoder position to get the position
+        m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
-    // relative to the chassis.
+        m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
-    return new SwerveModuleState(m_drivingEncoder.getVelocity(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
 
-  /**
+        m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
-   * Returns the current position of the module.
+            PersistMode.kPersistParameters);
-   *
-   * @return The current position of the module.
-   */
-  public SwerveModulePosition getPosition() {
-    // Apply chassis angular offset to the encoder position to get the position
-    // relative to the chassis.
-    return new SwerveModulePosition(
-        m_drivingEncoder.getPosition(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
 
-  /**
+        m_chassisAngularOffset = chassisAngularOffset;
-   * Sets the desired state for the module.
+        m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-   *
+        m_drive.setPosition(0);
-   * @param desiredState Desired state with speed and angle.
+    }
-   */
-  public void setDesiredState(SwerveModuleState desiredState) {
-    // Apply chassis angular offset to the desired state.
-    SwerveModuleState correctedDesiredState = new SwerveModuleState();
-    correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
-    correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
 
-    // Optimize the reference state to avoid spinning further than 90 degrees.
+    /**
-    correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
+     * Returns the current state of the module.
+     *
+     * @return The current state of the module.
+     */
+    public SwerveModuleState getState() {
+        // Apply chassis angular offset to the encoder position to get the position
+        // relative to the chassis.
+        return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+    }
 
-    // Command driving and turning SPARKS towards their respective setpoints.
+    /**
-    m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+     * Returns the current position of the module.
-    m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
+     *
+     * @return The current position of the module.
+     */
+    public SwerveModulePosition getPosition() {
+        // Apply chassis angular offset to the encoder position to get the position
+        // relative to the chassis.
+        return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+    }
 
-    m_desiredState = desiredState;
+    /**
-  }
+     * Sets the desired state for the module.
+     *
+     * @param desiredState Desired state with speed and angle.
+     */
+    public void setDesiredState(SwerveModuleState desiredState) {
+        // Apply chassis angular offset to the desired state.
+        SwerveModuleState correctedDesiredState = new SwerveModuleState();
+        correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
+        correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
 
-  public void setVoltageDrive(double voltage){
+        // Optimize the reference state to avoid spinning further than 90 degrees.
-    m_drivingSpark.setVoltage(voltage);
+        correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
-  }
 
-  public void setVoltageTurn(double voltage) {
+        // Command driving and turning SPARKS towards their respective setpoints.
-    m_turningSpark.setVoltage(voltage);
+        m_drive.setControl(
-  }
+        driveVelocityRequest.withVelocity(
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+        ).withFeedForward(
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+        )
+        );
 
-  public double getVoltageDrive() {
+        m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
-    return m_drivingSpark.get() * RobotController.getBatteryVoltage();
-  }
 
-  public double getVoltageTurn() {
+        m_desiredState = desiredState;
-    return m_turningSpark.get() * RobotController.getBatteryVoltage();
+    }
-  }
+
+    public void setVoltageDrive(double voltage){
+        m_drive.setVoltage(voltage);
+    }
+
+    public void setVoltageTurn(double voltage) {
+        m_turningSpark.setVoltage(voltage);
+    }
+
+    public double getVoltageDrive() {
+        return m_drive.get() * RobotController.getBatteryVoltage();
+    }
+
+    public double getVoltageTurn() {
+        return m_turningSpark.get() * RobotController.getBatteryVoltage();
+    }
+
+    public TalonFX getDrivingMotor(){
+        return m_drive;
+    }
+
+    /** Zeroes all the SwerveModule encoders. */
+    public void resetEncoders() {
+        m_drive.setPosition(0);
+    }
 
-  /** Zeroes all the SwerveModule encoders. */
-  public void resetEncoders() {
-    m_drivingEncoder.setPosition(0);
-  }
 }

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -1,18 +1,26 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IndexerConstants;
 import frc.robot.constants.ManipulatorConstants;
 
 public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
+    private SparkMax indexerMotor;
+
     private DigitalInput coralBeamBreak;
-    private DigitalInput algaeBeamBreak;
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -20,25 +28,116 @@ public class Manipulator extends SubsystemBase {
             MotorType.kBrushless
         );
 
+        manipulatorMotor.configure(
+            ManipulatorConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        indexerMotor = new SparkMax(
+            IndexerConstants.kIndexerMotorID,
+            MotorType.kBrushless
+        );
+
+        indexerMotor.configure(
+            IndexerConstants.motorConfig,
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
-        algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
-    public Command runManipulator(double speed) {
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("coral beam break", getCoralBeamBreak());
+    }
+
+    /**
+     * The default command for the manipulator that either stops the manipulator or slowly 
+     * runs the manipulator to retain the algae
+     * 
+     * @return Returns a command that sets the speed of the motor
+     */
+    public Command defaultCommand() {
         return run(() -> {
-            manipulatorMotor.set(speed);
+            runUntilCollected(() -> 0.1);
+            });
+    }
+
+    /**
+     * Runs the manipulator at a set speed with the direction based on the coral parameter
+     * 
+     * @param speed The speed at which the manipulator runs
+     * @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae)
+     * @return Returns a command that sets the speed of the motor
+     */
+    public Command runManipulator(DoubleSupplier speed, boolean coral) {
+        return run(() -> {
+            manipulatorMotor.set(
+                coral ? speed.getAsDouble() : speed.getAsDouble() * -1
+            );
+
+            indexerMotor.set(0);
         });
     }
 
-    public Command runUntilCollected(double speed, boolean coral) {
+    /**
+     * Runs the manipulator until either the algae or coral beam break reads true
+     * 
+     * @param speed The speed at which the manipulator is run
+     * @param coral Is the object a coral? (True = Coral, False = Algae)
+     * @return Returns a command that sets the speed of the motor
+     */
+    public Command runUntilCollected(DoubleSupplier speed) {
         return run(() -> {
-            manipulatorMotor.set(coral ? speed : speed * -1);
+            manipulatorMotor.setVoltage(
-        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
+                speed.getAsDouble()*12
+            );
+
+            indexerMotor.set(1);
+
+        }).unless(() -> !coralBeamBreak.get())
+        .until(() -> !coralBeamBreak.get());
+        /*
+        return run(() -> {
+            if(getCoralBeamBreak()) {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            }
+        */
     }
 
-    public Command indexCoral(double speed) {
+    public Command retractCommand(DoubleSupplier retractSpeed){
         return run(() -> {
-            manipulatorMotor.set(speed);
+            manipulatorMotor.set(-retractSpeed.getAsDouble());
-        }).until(coralBeamBreak::get);
+
+            indexerMotor.set(0);
+        }
+        ).until(() -> coralBeamBreak.get());
+    }
+
+    /**
+     * Runs the manipulator in a way that will bring the coral to a reliable holding position
+     * 
+     * @return Returns a command that will position the coral to a known location
+     */
+    public Command indexCoral() {
+        return run(() -> {
+            runUntilCollected(() -> 0.5)
+                .andThen(runManipulator(() -> .1, false))
+                .until(() -> getCoralBeamBreak());
+        });
+    }
+
+    public boolean getCoralBeamBreak() {
+        return coralBeamBreak.get();
     }
 }

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -0,0 +1,238 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ManipulatorPivotConstants;
+
+public class ManipulatorPivot extends SubsystemBase {
+    protected SparkMax pivotMotor;
+
+    private SparkAbsoluteEncoder encoder;
+
+    private ArmFeedforward feedForward;
+
+    private PIDController pidController;
+
+    private PIDController algaePIDController;
+
+    public ManipulatorPivot() {
+        pivotMotor = new SparkMax(
+            ManipulatorPivotConstants.kPivotMotorID, 
+            MotorType.kBrushless
+        );
+
+        pivotMotor.configure(
+            ManipulatorPivotConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+        
+        encoder = pivotMotor.getAbsoluteEncoder();
+
+        pidController = new PIDController(
+            ManipulatorPivotConstants.kPositionalP, 
+            ManipulatorPivotConstants.kPositionalI, 
+            ManipulatorPivotConstants.kPositionalD
+        );
+        pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+
+        pidController.setSetpoint(0);
+
+        pidController.enableContinuousInput(0, 280);
+        
+        algaePIDController = new PIDController(
+            ManipulatorPivotConstants.kAlgaeP,
+            0,
+            0);
+        algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+        
+        algaePIDController.setSetpoint(0);
+
+        algaePIDController.enableContinuousInput(0, 280);
+
+        feedForward = new ArmFeedforward(
+            ManipulatorPivotConstants.kFeedForwardS,
+            ManipulatorPivotConstants.kFeedForwardG, 
+            ManipulatorPivotConstants.kFeedForwardV
+            );
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("manipulator position", getEncoderPosition());
+        Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to the encoder's current position
+     * and the arm safe stow position
+     * 
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe() {
+        return isMotionSafe(getEncoderPosition());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to some target position and the
+     * arm safe stow position
+     * 
+     * @param motionTarget The target position to determine the safety of
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe(double motionTarget) {
+        return motionTarget > ManipulatorPivotConstants.kPivotSafeStowPosition;
+    }
+
+    /**
+     * Manual ManipulatorPivot command that sets the motor based on speed
+     * 
+     * @param speed The speed to set the motor
+     * @return A command that sets the motor speed
+     */
+    public Command runManualPivot(DoubleSupplier speed) {
+        return run(() -> {
+            pivotMotor.set(speed.getAsDouble());
+        });
+    }
+    
+    /**
+     * Moves the arm to a target destination (setpoint)
+     * 
+     * @param setpoint Target destination of the subsystem
+     * @param timeout Time to achieve the setpoint before quitting
+     * @return Sets motor voltage to achieve the target destination
+     */
+    public Command goToSetpoint(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> pidController.atSetpoint());
+    }
+
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            algaePIDController.setSetpoint(setpoint.getAsDouble());
+            algaePIDController.reset();
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    algaePIDController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> algaePIDController.atSetpoint());
+    }
+
+    public Command maintainPosition() {
+        return startRun(() -> {
+            
+        
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        pidController.getSetpoint()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            });
+    }
+
+    /**
+     * Returns the encoder's position in radians
+     * 
+     * @return Encoder's position in radians
+     */
+    public double getEncoderPosition() {
+        return Units.radiansToDegrees( encoder.getPosition());
+    }
+    /**
+     * Returns the encoder's velocity in radians per second
+     * 
+     * @return Encoder's velocity in radians per second
+     */
+    public double getEncoderVelocity() {
+        return Units.radiansToDegrees(encoder.getVelocity());
+    }
+
+    public double getCGPosition(){
+        return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
+    }
+
+    public double getPivotOutput(){
+        return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
+    }
+}

src/main/java/frc/robot/subsystems/Vision.java

@@ -0,0 +1,187 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.VisionConstants;
+
+public class Vision{
+
+    private NetworkTable blackVisionTable;
+
+    private DoubleSubscriber black_tx;
+    private DoubleSubscriber black_ty;
+    private DoubleSubscriber black_dist;
+
+    private DoubleSubscriber blackClosestTag;
+    private BooleanSubscriber blackTagDetected;
+
+    private DoubleSubscriber blackFramerate;
+
+    private NetworkTable orangeVisionTable;
+
+    private DoubleSubscriber orange_tx;
+    private DoubleSubscriber orange_ty;
+    private DoubleSubscriber orange_dist;
+
+    private DoubleSubscriber orangeClosestTag;
+    private BooleanSubscriber orangeTagDetected;
+
+    private DoubleSubscriber orangeFramerate;
+
+    private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 14.0-1.05, 7.308+2.75};
+    private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
+
+    public Vision(){
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+        blackVisionTable = inst.getTable("black_Fiducial");
+        orangeVisionTable = inst.getTable("orange_Fiducial");
+
+        black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
+
+        blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
+        blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
+
+        blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
+
+        orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
+
+        orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
+        orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
+
+        orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
+    
+    }
+    
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
+        Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
+
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
+
+        Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
+
+        Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
+
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
+    }
+
+    public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
+
+       // System.out.println(gyroBuffer.getSample(timestamp));
+
+        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
+
+        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
+
+        Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
+        Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
+            new Rotation2d());
+
+        Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
+            .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
+            .getTranslation();
+        Pose2d robotPose = new Pose2d(
+            fieldToCameraTranslation,
+            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
+
+        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
+
+        return robotPose;
+    }
+
+    public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
+            getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
+    }
+
+    public double getBlackTX(){
+        return black_tx.get();
+    }
+
+    public double getBlackTY(){
+        return black_ty.get();
+    }
+
+    public double getBlackDist(){
+        return black_dist.get();
+    }
+
+    public int getBlackClosestTag(){
+        return (int) blackClosestTag.get();
+    }
+
+    public double getBlackTimeStamp(){
+        return black_tx.getLastChange()-VisionConstants.latencyFudge;
+    }
+
+    public boolean getBlackTagDetected(){
+        return blackTagDetected.get();
+    }
+
+    public double getBlackFPS(){
+        return blackFramerate.get();
+    }
+
+    public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
+            return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+                orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
+                );
+        }else{
+            return new Pose2d();
+        }
+    }
+    
+    public double getOrangeTX(){
+        return orange_tx.get();
+    }
+
+    public double getOrangeTY(){
+        return orange_ty.get();
+    }
+
+    public double getOrangeDist(){
+        return orange_dist.get();
+    }
+
+    public int getOrangeClosestTag(){
+        return (int) orangeClosestTag.get();
+    }
+
+    public double getOrangeTimeStamp(){
+        return orange_tx.getLastChange()-VisionConstants.latencyFudge;
+    }
+
+    public boolean getOrangeTagDetected(){
+        return orangeTagDetected.get();
+    }
+
+    public double getOrangeFPS(){
+        return orangeFramerate.get();
+    }
+
+    public boolean isBlackConnected(){
+        return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
+    }
+
+    public boolean isOrangeConnected(){
+        return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
+    }
+    
+}

src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java

@@ -1,8 +1,8 @@
 package frc.robot.subsystems.sysid;
 
-import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
 import edu.wpi.first.units.measure.MutLinearVelocity;
@@ -27,9 +27,9 @@ public class ElevatorSysID extends Elevator {
 
         appliedVoltage = Volts.mutable(0);
 
-        elevatorPosition = Meters.mutable(0);
+        elevatorPosition = Inches.mutable(0);
 
-        elevatorVelocity = MetersPerSecond.mutable(0);
+        elevatorVelocity = InchesPerSecond.mutable(0);
 
         routine = new SysIdRoutine(
             ElevatorConstants.kSysIDConfig,
@@ -41,10 +41,10 @@ public class ElevatorSysID extends Elevator {
                             elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
-                            encoder.getPosition(), Meters
+                            encoder.getPosition(), Inches
                         ))
                         .linearVelocity(elevatorVelocity.mut_replace(
-                            encoder.getVelocity(), MetersPerSecond
+                            encoder.getVelocity(), InchesPerSecond
                         ));
                 }, 
                 this
@@ -52,6 +52,11 @@ public class ElevatorSysID extends Elevator {
         );
     }
 
+    @Override
+    public void periodic() {
+
+    }
+
     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
         return routine.quasistatic(direction);
     }

src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.subsystems.ManipulatorPivot;
+
+public class ManipulatorPivotSysID extends ManipulatorPivot {
+    private MutVoltage appliedVoltage;
+
+    private MutAngle pivotPosition;
+
+    private MutAngularVelocity pivotVelocity;
+
+    private SysIdRoutine routine;
+
+    public ManipulatorPivotSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        pivotPosition = Radians.mutable(0);
+
+        pivotVelocity = RadiansPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ManipulatorPivotConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                pivotMotor::setVoltage, 
+                (log) -> {
+                    log.motor("armMotor")
+                        .voltage(appliedVoltage.mut_replace(
+                            pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .angularPosition(pivotPosition.mut_replace(
+                            getEncoderPosition(), Radians
+                        ))
+                        .angularVelocity(pivotVelocity.mut_replace(
+                            getEncoderVelocity(), RadiansPerSecond
+                        ));
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+}

vendordeps/AdvantageKit.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.1",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+    ],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.1",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}

vendordeps/PathplannerLib-2025.2.1.json

@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2025.2.1.json",
+    "name": "PathplannerLib",
+    "version": "2025.2.1",
+    "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.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2025.2.1",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}

vendordeps/Phoenix5-5.35.1.json

@@ -1,50 +1,50 @@
 {
-    "fileName": "Phoenix5-5.34.0-beta-4.json",
+    "fileName": "Phoenix5-5.35.1.json",
     "name": "CTRE-Phoenix (v5)",
-    "version": "5.34.0-beta-4",
+    "version": "5.35.1",
     "frcYear": "2025",
     "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
     "mavenUrls": [
         "https://maven.ctr-electronics.com/release/"
     ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json",
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json",
     "requires": [
         {
             "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
             "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
-            "offlineFileName": "Phoenix6-frc2025-beta-latest.json",
+            "offlineFileName": "Phoenix6-frc2025-latest.json",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json"
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json"
         }
     ],
     "conflictsWith": [
         {
             "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
             "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.",
-            "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json"
+            "offlineFileName": "Phoenix6-replay-frc2025-latest.json"
         },
         {
             "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
             "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
-            "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json"
+            "offlineFileName": "Phoenix5-replay-frc2025-latest.json"
         }
     ],
     "javaDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-java",
-            "version": "5.34.0-beta-4"
+            "version": "5.35.1"
         },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-java",
-            "version": "5.34.0-beta-4"
+            "version": "5.35.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -58,7 +58,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "cci-sim",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -74,7 +74,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-cpp",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_Phoenix_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -90,7 +90,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-cpp",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_Phoenix",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -106,7 +106,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_PhoenixCCI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -122,7 +122,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_Phoenix_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "api-cpp-sim",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_PhoenixSim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -154,7 +154,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "cci-sim",
-            "version": "5.34.0-beta-4",
+            "version": "5.35.1",
             "libName": "CTRE_PhoenixCCISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,

vendordeps/Phoenix6-25.2.1.json

@@ -1,32 +1,32 @@
 {
-    "fileName": "Phoenix6-25.0.0-beta-4.json",
+    "fileName": "Phoenix6-25.2.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.0.0-beta-4",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
         "https://maven.ctr-electronics.com/release/"
     ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json",
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json",
     "conflictsWith": [
         {
             "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
             "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
-            "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json"
+            "offlineFileName": "Phoenix6-replay-frc2025-latest.json"
         }
     ],
     "javaDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.0.0-beta-4"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,21 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANrange",
-            "version": "25.0.0-beta-4",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANrange",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -355,9 +385,9 @@
         },
         {
             "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANrange",
+            "artifactId": "simProPigeon2",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
-            "libName": "CTRE_SimProCANrange",
+            "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -371,9 +401,9 @@
         },
         {
             "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
+            "artifactId": "simProCANrange",
-            "version": "25.0.0-beta-4",
+            "version": "25.2.1",
-            "libName": "CTRE_SimProPigeon2",
+            "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,

vendordeps/REVLib-2025.0.1.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.0-beta-3.json",
+    "fileName": "REVLib-2025.0.1.json",
     "name": "REVLib",
-    "version": "2025.0.0-beta-3",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0-beta-3"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0-beta-3",
+            "version": "2025.0.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0-beta-3",
+            "version": "2025.0.1",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0-beta-3",
+            "version": "2025.0.1",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",

vendordeps/Studica-2025.0.1.json

@@ -0,0 +1,71 @@
+{
+    "fileName": "Studica-2025.0.1.json",
+    "name": "Studica",
+    "version": "2025.0.1",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2025/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
+    "cppDependencies": [
+        {
+            "artifactId": "Studica-cpp",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "Studica",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        },
+        {
+            "artifactId": "Studica-driver",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "StudicaDriver",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "artifactId": "Studica-java",
+            "groupId": "com.studica.frc",
+            "version": "2025.0.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "artifactId": "Studica-driver",
+            "groupId": "com.studica.frc",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "version": "2025.0.1"
+        }
+    ]
+}