Merge branch 'main' into kraken_swerve

Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code
Adding basic PathPlanner setup
2025-01-20 20:12:40 -05:00 · 2025-01-20 20:02:53 -05:00 · 2025-01-20 20:02:51 -05:00 · 2025-01-21 00:58:38 +00:00 · 2025-01-20 19:33:33 -05:00 · 2025-01-18 16:04:54 -05:00
24 changed files with 1144 additions and 443 deletions
--- a/WPILib-License.md
+++ b/WPILib-License.md
@@ -1,24 +1,24 @@
-Copyright (c) 2009-2024 FIRST and other WPILib contributors
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-   * Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   * Redistributions in binary form must reproduce the above copyright
-     notice, this list of conditions and the following disclaimer in the
-     documentation and/or other materials provided with the distribution.
-   * Neither the name of FIRST, WPILib, nor the names of other WPILib
-     contributors may be used to endorse or promote products derived from
-     this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--- a/build.gradle
+++ b/build.gradle
@@ -1,102 +1,104 @@
-plugins {
-    id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
-}
-
-java {
-    sourceCompatibility = JavaVersion.VERSION_17
-    targetCompatibility = JavaVersion.VERSION_17
-}
-
-def ROBOT_MAIN_CLASS = "frc.robot.Main"
-
-// Define my targets (RoboRIO) and artifacts (deployable files)
-// This is added by GradleRIO's backing project DeployUtils.
-deploy {
-    targets {
-        roborio(getTargetTypeClass('RoboRIO')) {
-            // Team number is loaded either from the .wpilib/wpilib_preferences.json
-            // or from command line. If not found an exception will be thrown.
-            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
-            // want to store a team number in this file.
-            team = project.frc.getTeamNumber()
-            debug = project.frc.getDebugOrDefault(false)
-
-            artifacts {
-                // First part is artifact name, 2nd is artifact type
-                // getTargetTypeClass is a shortcut to get the class type using a string
-
-                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
-                }
-
-                // Static files artifact
-                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
-                    files = project.fileTree('src/main/deploy')
-                    directory = '/home/lvuser/deploy'
-                }
-            }
-        }
-    }
-}
-
-def deployArtifact = deploy.targets.roborio.artifacts.frcJava
-
-// Set to true to use debug for JNI.
-wpi.java.debugJni = false
-
-// Set this to true to enable desktop support.
-def includeDesktopSupport = false
-
-// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-// Also defines JUnit 5.
-dependencies {
-    annotationProcessor wpi.java.deps.wpilibAnnotations()
-    implementation wpi.java.deps.wpilib()
-    implementation wpi.java.vendor.java()
-
-    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
-    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
-
-    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
-    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
-
-    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
-    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
-    simulationDebug wpi.sim.enableDebug()
-
-    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
-    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
-    simulationRelease wpi.sim.enableRelease()
-
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-}
-
-test {
-    useJUnitPlatform()
-    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
-}
-
-// Simulation configuration (e.g. environment variables).
-wpi.sim.addGui().defaultEnabled = true
-wpi.sim.addDriverstation()
-
-// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
-// in order to make them all available at runtime. Also adding the manifest so WPILib
-// knows where to look for our Robot Class.
-jar {
-    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
-    from sourceSets.main.allSource
-    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
-    duplicatesStrategy = DuplicatesStrategy.INCLUDE
-}
-
-// Configure jar and deploy tasks
-deployArtifact.jarTask = jar
-wpi.java.configureExecutableTasks(jar)
-wpi.java.configureTestTasks(test)
-
-// Configure string concat to always inline compile
-tasks.withType(JavaCompile) {
-    options.compilerArgs.add '-XDstringConcat=inline'
-}
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+}
+
+java {
+    sourceCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
+}
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamNumber()
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                                           // longer exist in deploy directory of this project
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = false
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 5.
+dependencies {
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+}
+
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    from sourceSets.main.allSource
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,7 +1,7 @@
-distributionBase=GRADLE_USER_HOME
-distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip
-networkTimeout=10000
-validateDistributionUrl=true
-zipStoreBase=GRADLE_USER_HOME
-zipStorePath=permwrapper/dists
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -1,94 +1,94 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-@rem SPDX-License-Identifier: Apache-2.0
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
--- a/settings.gradle
+++ b/settings.gradle
@@ -1,30 +1,30 @@
-import org.gradle.internal.os.OperatingSystem
-
-pluginManagement {
-    repositories {
-        mavenLocal()
-        gradlePluginPortal()
-        String frcYear = '2025'
-        File frcHome
-        if (OperatingSystem.current().isWindows()) {
-            String publicFolder = System.getenv('PUBLIC')
-            if (publicFolder == null) {
-                publicFolder = "C:\\Users\\Public"
-            }
-            def homeRoot = new File(publicFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
-        } else {
-            def userFolder = System.getProperty("user.home")
-            def homeRoot = new File(userFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
-        }
-        def frcHomeMaven = new File(frcHome, 'maven')
-        maven {
-            name 'frcHome'
-            url frcHomeMaven
-        }
-    }
-}
-
-Properties props = System.getProperties();
-props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2025'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name = 'frcHome'
+            url = frcHomeMaven
+        }
+    }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,8 @@

 package frc.robot;

+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.ClimberPivot;
@@ -12,8 +14,16 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
+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 {
@@ -34,6 +44,8 @@ public class RobotContainer {
 	private CommandXboxController driver;
 	private CommandXboxController operator;

+	private SendableChooser<Command> autoChooser;
+
 	public RobotContainer() {
 		arm = new Arm();

@@ -52,7 +64,13 @@ public class RobotContainer {
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);

+		autoChooser = AutoBuilder.buildAutoChooser();
+
 		configureButtonBindings();
+
+		configureNamedCommands();
+
+		configureShuffleboard();
 	}

 	private void configureButtonBindings() {
@@ -61,7 +79,7 @@ public class RobotContainer {
 		);

 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0)
+			climberPivot.goToAngle(0, 1)
 		);

 		climberRollers.setDefaultCommand(
@@ -78,7 +96,7 @@ public class RobotContainer {
 		);

 		elevator.setDefaultCommand(
-			elevator.goToSetpoint(0, 1)
+			elevator.runAssistedElevator(operator::getLeftY)
 		);

 		indexer.setDefaultCommand(
@@ -100,23 +118,140 @@ public class RobotContainer {

 		//Operator inputs
 		operator.povUp().onTrue(
-			elevator.goToSetpoint(0, 0)
+			moveManipulator(
+				ElevatorConstants.kElevatorL4Position, 
+				ArmConstants.kArmL4Position
+			)
 		);

 		operator.povRight().onTrue(
-			elevator.goToSetpoint(0, 0)
+			moveManipulator(
+				ElevatorConstants.kElevatorL3Position, 
+				ArmConstants.kArmL3Position
+			)
 		);

 		operator.povLeft().onTrue(
-			elevator.goToSetpoint(0, 0)
+			moveManipulator(
+				ElevatorConstants.kElevatorL2Position, 
+				ArmConstants.kArmL2Position
+			)
 		);

 		operator.povDown().onTrue(
-			elevator.goToSetpoint(0, 0)
+			moveManipulator(
+				ElevatorConstants.kElevatorL1Position, 
+				ArmConstants.kArmL1Position
+			)
+		);
+
+		operator.a().onTrue(
+			coralIntakeRoutine()
+		);
+
+		operator.x().onTrue(
+			algaeIntakeRoutine(true)
+		);
+
+		operator.b().onTrue(
+			algaeIntakeRoutine(false)
 		);
 	}

-	public Command getAutonomousCommand() {
-		return new PrintCommand("NO AUTO DEFINED");
+	private void configureNamedCommands() {
+		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
 	}
-}
+
+	//creates tabs and transforms them on the shuffleboard
+    private void configureShuffleboard() {
+		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
+    	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+
+		Shuffleboard.selectTab(OIConstants.kAutoTab);
+
+		autoTab.add("Auto Selection", autoChooser)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kComboBoxChooser);
+
+		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(2, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+  }
+
+	public Command getAutonomousCommand() {
+		return autoChooser.getSelected();
+	}
+
+	//teleop routines
+	private Command coralIntakeRoutine() {
+		return moveManipulator(
+			ElevatorConstants.kElevatorCoralIntakePosition, 
+			ArmConstants.kArmCoralIntakePosition
+		)
+			.andThen(manipulator.runUntilCollected(1, true));
+	}
+
+	private Command algaeIntakeRoutine(boolean l2) {
+		return moveManipulator(
+			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
+			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
+		)
+			.andThen(manipulator.runUntilCollected(1, false));
+	}
+
+	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)) || (arm.isMotionSafe() && arm.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 (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { 
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+				.andThen(arm.goToSetpoint(armPosition, 2));
+		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
+		} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
+			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
+		// If the arm is behind the brace, move the arm first, then the elevator
+		} else if (!arm.isMotionSafe()) {
+			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
+		// Catch all command that's safe regardless of arm and elevator positions
+		} else { 
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+				.andThen(arm.goToSetpoint(armPosition, 2));
+		}
+	}
+
+	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+		return Commands.either(
+			Commands.either(
+				elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
+				() -> sequential
+			), 
+			Commands.either(
+				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
+				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				() -> sequential
+			), 
+			() -> elevatorFirst
+		);
+	}
+
+	/*
+	 * A moveManipulator method that will guarantee a safe movement. 
+	 * Here in case we need want to skip moveManipulator debugging
+	 */
+	@SuppressWarnings("unused")
+	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+			.andThen(arm.goToSetpoint(armPosition, 2));
+	}
+}
--- a/src/main/java/frc/robot/constants/ArmConstants.java
+++ b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -1,8 +1,79 @@
 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.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+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 ArmConstants {
    public static final int kArmMotorID = 0;
    public static final int kCANcoderID = 0;

-    public static final double kEncoderConversionFactor = 0;
+    public static final int kMotorAmpsMax = 0;
+
+    public static final double kArmMaxVelocity = 0;
+
+    public static final double kPositionalP = 0;
+    public static final double kPositionalI = 0;
+    public static final double kPositionalD = 0;
+    public static final double kPositionalTolerance = Units.degreesToRadians(1);
+
+    public static final double kVelocityP = 0;
+    public static final double kVelocityI = 0;
+    public static final double kVelocityD = 0;
+    // TODO Is this reasonable?
+    public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
+
+    public static final double kArmCoralIntakePosition = 0;
+    public static final double kArmL1Position = 0;
+    public static final double kArmL2Position = 0;
+    public static final double kArmL3Position = 0;
+    public static final double kArmL4Position = 0;
+    public static final double kArmL2AlgaePosition = 0;
+    public static final double kArmL3AlgaePosition = 0;
+    public static final double kArmSafeStowPosition = 0;
+
+    public static final double kMagnetOffset = 0.0;
+    public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+    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 CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+        canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
+        // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
+        //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+
+        motorConfig
+            .smartCurrentLimit(kMotorAmpsMax)
+            .idleMode(kIdleMode);
+    }
+
+
 }
--- a/src/main/java/frc/robot/constants/AutoConstants.java
+++ b/src/main/java/frc/robot/constants/AutoConstants.java
@@ -1,5 +1,13 @@
 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 {
@@ -15,4 +23,21 @@ public class AutoConstants {
    // Constraint for the motion profiled robot angle controller
    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = 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(kPXController, 0, 0),
+        new PIDConstants(kPYController, 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();
+        }
+    }
 }
--- a/src/main/java/frc/robot/constants/ElevatorConstants.java
+++ b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -11,20 +11,40 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;

 public class ElevatorConstants {
-    public static final int kElevatorMotorID = 0;
+    public static final int kElevatorMotor1ID = 0;
+    public static final int kElevatorMotor2ID = 0;
+
+    public static final int kTopLimitSwitchID = 0;
+    public static final int kBottomLimitSwitchID = 0;

    public static final double kEncoderConversionFactor = 0;

    public static final int kMotorAmpsMax = 0;

-    public static final double kPIDControllerP = 0;
-    public static final double kPIDControllerI = 0;
-    public static final double kPIDControllerD = 0;
+    public static final double kPositionControllerP = 0;
+    public static final double kPositionControllerI = 0;
+    public static final double kPositionControllerD = 0;
+    
+    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 kFeedForwardG = 0;
    public static final double kFeedForwardV = 0;

+    public static final double kElevatorMaxVelocity = 0;
+
+    public static final double kElevatorCoralIntakePosition = 0;
+    public static final double kElevatorL1Position = 0;
+    public static final double kElevatorL2Position = 0;
+    public static final double kElevatorL3Position = 0;
+    public static final double kElevatorL4Position = 0;
+    public static final double kElevatorL2AlgaePosition = 0;
+    public static final double kElevatorL3AlgaePosition = 0;
+    public static final double kElevatorBracePosition = 0;
+    public static final double kElevatorMaxHeight = 0;
+
    // 1, 7, 10 are the defaults for these, change as necessary
    public static final double kSysIDRampRate = 1;
    public static final double kSysIDStepVolts = 7;
--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -2,6 +2,12 @@ package frc.robot.constants;

 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+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 {
@@ -20,52 +26,73 @@ public class ModuleConstants {
    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 = 120;
    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 {
-        // Use module constants to calculate conversion factors and feed forward gain.
-        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
-        double turningFactor = 2 * Math.PI;
-        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();

-        drivingConfig
-            .idleMode(IdleMode.kBrake)
-            .smartCurrentLimit(kDriveMotorCurrentLimit);
-        drivingConfig.encoder
-            .positionConversionFactor(drivingFactor) // meters
-            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
-        drivingConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-            // These are example gains you may need to them for your own robot!
-            .pid(0.04, 0, 0)
-            .velocityFF(drivingVelocityFeedForward)
-            .outputRange(-1, 1);
+    static {
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
+
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
+
+        kDriveMotorConfig.Inverted = kDriveInversionState;
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
+
+        kDriveSlot0Config.kP = kDriveP;
+        kDriveSlot0Config.kI = kDriveI;
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;

        turningConfig
-            .idleMode(IdleMode.kBrake)
-            .smartCurrentLimit(20);
+            .idleMode(kTurnIdleMode)
+            .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
-            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+            .positionConversionFactor(kTurningFactor) // radians
+            .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);
    }
 }
--- a/src/main/java/frc/robot/constants/OIConstants.java
+++ b/src/main/java/frc/robot/constants/OIConstants.java
@@ -5,4 +5,7 @@ public class OIConstants {
    public static final int kOperatorControllerPort = 1;
    
    public static final double kDriveDeadband = 0.05;
+
+    public static final String kAutoTab = "Auto Tab";
+    public static final String kSensorsTab = "Sensors Tab";
 }
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -1,43 +1,117 @@
 package frc.robot.subsystems;

+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.hardware.CANcoder;
 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.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.ArmConstants;

 public class Arm extends SubsystemBase {
-    private SparkMax ArmMotor;
+    protected SparkMax armMotor;

    private CANcoder canCoder;

-    private PIDController pidController;
+    private PIDController positionController;
+    private PIDController velocityController;
+
    private ArmFeedforward feedForward;

    public Arm() {
-        ArmMotor = new SparkMax(
+        armMotor = new SparkMax(
            ArmConstants.kArmMotorID, 
            MotorType.kBrushless
        );

+        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        positionController = new PIDController(
+            ArmConstants.kPositionalP, 
+            ArmConstants.kPositionalI, 
+            ArmConstants.kPositionalD
+        );
+        
+        // TODO: Generate constants for continuous input range based on CANcoder configuration?
+        positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
+        positionController.setTolerance(ArmConstants.kPositionalTolerance);
+
+        velocityController = new PIDController(
+            ArmConstants.kVelocityP, 
+            ArmConstants.kVelocityI, 
+            ArmConstants.kVelocityD
+        );
+
+        velocityController.setTolerance(ArmConstants.kVelocityTolerance);
+
        canCoder = new CANcoder(ArmConstants.kCANcoderID);
+        canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
+    }
+
+    /**
+     * 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 > ArmConstants.kArmSafeStowPosition;
+    }
+
+    //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(
+                getEncoderVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(
+                getEncoderPosition(), 
+                getEncoderVelocity()
+            );
+
+            armMotor.setVoltage(voltsOut);
+        });
    }

    public Command goToSetpoint(double setpoint, double timeout) {
        return run(() -> {
-            double voltsOut = pidController.calculate(
-                canCoder.getPosition().getValueAsDouble(), 
+            double voltsOut = positionController.calculate(
+                getEncoderPosition(), 
                setpoint
            ) + feedForward.calculate(
-                canCoder.getPosition().getValueAsDouble(), 
-                canCoder.getVelocity().getValueAsDouble()
+                getEncoderPosition(), 
+                getEncoderVelocity()
            );

-            ArmMotor.setVoltage(voltsOut);
-        }).until(pidController::atSetpoint).withTimeout(timeout);
+            armMotor.setVoltage(voltsOut);
+        }).until(positionController::atSetpoint).withTimeout(timeout);
+    }
+
+    public double getEncoderPosition() {
+        return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
+    }
+
+    public double getEncoderVelocity() {
+        return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
    }
 }
--- a/src/main/java/frc/robot/subsystems/ClimberPivot.java
+++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems;

-import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;

@@ -13,7 +13,7 @@ import frc.robot.constants.ClimberPivotConstants;
 public class ClimberPivot extends SubsystemBase {
    private SparkMax pivotMotor;

-    private AbsoluteEncoder neoEncoder;
+    private RelativeEncoder neoEncoder;

    private DigitalInput cageLimitSwitch;

@@ -25,9 +25,9 @@ public class ClimberPivot extends SubsystemBase {
            MotorType.kBrushless
        );

-        neoEncoder = pivotMotor.getAbsoluteEncoder();
+        neoEncoder = pivotMotor.getEncoder();

-        cageLimitSwitch = new DigitalInput(0);
+        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);

        pidController = new PIDController(
            ClimberPivotConstants.kPIDControllerP,
@@ -42,7 +42,7 @@ public class ClimberPivot extends SubsystemBase {
        });
    }

-    public Command goToAngle(double setpoint) {
+    public Command goToAngle(double setpoint, double timeout) {
        return run(() -> {
            pivotMotor.set(
                pidController.calculate(
@@ -50,7 +50,7 @@ public class ClimberPivot extends SubsystemBase {
                    setpoint
                )
            );
-        });
+        }).withTimeout(timeout);
    }

    public boolean getCageLimitSwitch() {
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -4,9 +4,14 @@

 package frc.robot.subsystems;

+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;

+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.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -15,10 +20,10 @@ 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.wpilibj.ADIS16470_IMU.IMUAxis;
+import edu.wpi.first.wpilibj.DriverStation;
 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;

@@ -30,7 +35,7 @@ public class Drivetrain extends SubsystemBase {
    protected MAXSwerveModule m_rearRight;

    // The gyro sensor
-    private ADIS16470_IMU m_gyro;
+    private AHRS ahrs;

    // Odometry class for tracking robot pose
    private SwerveDriveOdometry m_odometry;
@@ -61,11 +66,41 @@ public class Drivetrain extends SubsystemBase {
            DrivetrainConstants.kBackRightChassisAngularOffset
        );

-        m_gyro = new ADIS16470_IMU();
+        ahrs = new AHRS(NavXComType.kMXP_SPI);

        m_odometry = new SwerveDriveOdometry(
            DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(ahrs.getAngle()),
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+        });
+
+        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
+        );
+    }
+
+    @Override
+    public void periodic() {
+        // Update the odometry in the periodic block
+        m_odometry.update(
+            Rotation2d.fromDegrees(getGyroValue()),
            new SwerveModulePosition[] {
                m_frontLeft.getPosition(),
                m_frontRight.getPosition(),
@@ -74,17 +109,21 @@ public class Drivetrain extends SubsystemBase {
        });
    }

-    @Override
-    public void periodic() {
-        // Update the odometry in the periodic block
-        m_odometry.update(
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
-            new SwerveModulePosition[] {
-                m_frontLeft.getPosition(),
-                m_frontRight.getPosition(),
-                m_rearLeft.getPosition(),
-                m_rearRight.getPosition()
-        });
+    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);
    }

    /**
@@ -103,7 +142,7 @@ public class Drivetrain extends SubsystemBase {
     */
    public void resetOdometry(Pose2d pose) {
        m_odometry.resetPosition(
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(getGyroValue()),
            new SwerveModulePosition[] {
                m_frontLeft.getPosition(),
                m_frontRight.getPosition(),
@@ -144,7 +183,7 @@ public class Drivetrain extends SubsystemBase {
        var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
            fieldRelative
                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
+                        Rotation2d.fromDegrees(getGyroValue()))
                : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
        SwerveDriveKinematics.desaturateWheelSpeeds(
            swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -194,7 +233,11 @@ public class Drivetrain extends SubsystemBase {

    /** Zeroes the heading of the robot. */
    public void zeroHeading() {
-        m_gyro.reset();
+        ahrs.reset();;
+    }
+
+    public double getGyroValue() {
+        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
    }

    /**
@@ -203,7 +246,7 @@ 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 Rotation2d.fromDegrees(getGyroValue()).getDegrees();
    }

    /**
@@ -212,6 +255,6 @@ 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 ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
    }
 }
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems;

+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -8,22 +10,32 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;

 import edu.wpi.first.math.controller.ElevatorFeedforward;
 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.ElevatorConstants;

 public class Elevator extends SubsystemBase {
    protected SparkMax elevatorMotor1;
+    protected SparkMax elevatorMotor2;

    protected RelativeEncoder encoder;

-    private PIDController pidController;
+    private DigitalInput bottomLimitSwitch;
+
+    private PIDController positionController;
+    private PIDController velocityController;

    private ElevatorFeedforward feedForward;
    
    public Elevator() {
        elevatorMotor1 = new SparkMax(
-            ElevatorConstants.kElevatorMotorID, 
+            ElevatorConstants.kElevatorMotor1ID, 
+            MotorType.kBrushless
+        );
+        
+        elevatorMotor2 = new SparkMax(
+            ElevatorConstants.kElevatorMotor2ID,
            MotorType.kBrushless
        );

@@ -33,12 +45,28 @@ public class Elevator extends SubsystemBase {
            PersistMode.kPersistParameters
        );

+        elevatorMotor2.configure(
+            ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID), 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
        encoder = elevatorMotor1.getEncoder();

-        pidController = new PIDController(
-            ElevatorConstants.kPIDControllerP, 
-            ElevatorConstants.kPIDControllerI, 
-            ElevatorConstants.kPIDControllerD
+        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(
@@ -48,20 +76,94 @@ public class Elevator extends SubsystemBase {
        );
    }

-    public Command runElevator(double speed) {
+    /**
+     * 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.kElevatorBracePosition;
+    }
+
+    /**
+     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed How fast the elevator moves
+     * @return Sets motor voltage to move the elevator relative to the speed parameter
+     */
+    public Command runAssistedElevator(DoubleSupplier speed) {
+        return run(() -> {
+            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                encoder.getVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(realSpeedTarget);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
+    }
+
+    /**
+     * 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(double speed) {
        return run(() -> {
            elevatorMotor1.set(speed);
        });
    }
    
+    /**
+     * Moves the elevator 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(double setpoint, double timeout) {
        return run(() -> {
-            double voltsOut = pidController.calculate(
+            double voltsOut = positionController.calculate(
                encoder.getPosition(),
                setpoint
            ) + feedForward.calculate(0);

            elevatorMotor1.setVoltage(voltsOut);
-        }).until(pidController::atSetpoint).withTimeout(timeout);
+        }).until(
+            () -> positionController.atSetpoint() || bottomLimitSwitch.get()
+        ).withTimeout(timeout);
    }
-}
+
+    /**
+     * 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();
+    }
+}
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -31,7 +31,6 @@ public class Indexer extends SubsystemBase {
    public Command indexCoral(double speed) {
        return run(() -> {
            indexerMotor.set(speed);
-        })
-        .until(indexerBeamBreak::get);
+        }).until(indexerBeamBreak::get);
    }
 }
--- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
@@ -15,21 +15,22 @@ 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 RelativeEncoder m_drivingEncoder;
  private final AbsoluteEncoder m_turningEncoder;

-  private final SparkClosedLoopController m_drivingClosedLoopController;
  private final SparkClosedLoopController m_turningClosedLoopController;

+  private final VelocityVoltage driveVelocityRequest;
+
  private double m_chassisAngularOffset = 0;
  private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());

@@ -40,26 +41,29 @@ public class MAXSwerveModule {
   * Encoder.
   */
  public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
+    m_drive = new TalonFX(drivingCANId);
    m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);

-    m_drivingEncoder = m_drivingSpark.getEncoder();
    m_turningEncoder = m_turningSpark.getAbsoluteEncoder();

-    m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
    m_turningClosedLoopController = m_turningSpark.getClosedLoopController();

+    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+
    // Apply the respective configurations to the SPARKS. Reset parameters before
    // 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_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
    m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
        PersistMode.kPersistParameters);

    m_chassisAngularOffset = chassisAngularOffset;
    m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
  }

  /**
@@ -70,7 +74,7 @@ public class MAXSwerveModule {
  public SwerveModuleState getState() {
    // Apply chassis angular offset to the encoder position to get the position
    // relative to the chassis.
-    return new SwerveModuleState(m_drivingEncoder.getVelocity(),
+    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
  }

@@ -82,8 +86,7 @@ public class MAXSwerveModule {
  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(),
+    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
  }

@@ -102,14 +105,21 @@ public class MAXSwerveModule {
    correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));

    // Command driving and turning SPARKS towards their respective setpoints.
-    m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+    m_drive.setControl(
+      driveVelocityRequest.withVelocity(
+        correctedDesiredState.speedMetersPerSecond
+      ).withFeedForward(
+        correctedDesiredState.speedMetersPerSecond
+      )
+    );
+
    m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);

    m_desiredState = desiredState;
  }

  public void setVoltageDrive(double voltage){
-    m_drivingSpark.setVoltage(voltage);
+    m_drive.setVoltage(voltage);
  }

  public void setVoltageTurn(double voltage) {
@@ -117,7 +127,7 @@ public class MAXSwerveModule {
  }

  public double getVoltageDrive() {
-    return m_drivingSpark.get() * RobotController.getBatteryVoltage();
+    return m_drive.get() * RobotController.getBatteryVoltage();
  }

  public double getVoltageTurn() {
@@ -126,6 +136,6 @@ public class MAXSwerveModule {

  /** Zeroes all the SwerveModule encoders. */
  public void resetEncoders() {
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
  }
 }
--- a/src/main/java/frc/robot/subsystems/Manipulator.java
+++ b/src/main/java/frc/robot/subsystems/Manipulator.java
@@ -33,14 +33,6 @@ public class Manipulator extends SubsystemBase {
    public Command runUntilCollected(double speed, boolean coral) {
        return run(() -> {
            manipulatorMotor.set(coral ? speed : speed * -1);
-        })
-        .until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
-    }
-
-    public Command indexCoral(double speed) {
-        return run(() -> {
-            manipulatorMotor.set(speed);
-        })
-        .until(coralBeamBreak::get);
+        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
    }
 }
--- a/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java
+++ b/src/main/java/frc/robot/subsystems/sysid/ArmSysID.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.ArmConstants;
+import frc.robot.subsystems.Arm;
+
+public class ArmSysID extends Arm {
+    private MutVoltage appliedVoltage;
+
+    private MutAngle armPosition;
+
+    private MutAngularVelocity armVelocity;
+
+    private SysIdRoutine routine;
+
+    public ArmSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        armPosition = Radians.mutable(0);
+
+        armVelocity = RadiansPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ArmConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                armMotor::setVoltage, 
+                (log) -> {
+                    log.motor("armMotor")
+                        .voltage(appliedVoltage.mut_replace(
+                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .angularPosition(armPosition.mut_replace(
+                            getEncoderPosition(), Radians
+                        ))
+                        .angularVelocity(armVelocity.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);
+    }
+}
--- a/vendordeps/PathplannerLib-2025.2.1.json
+++ b/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"
+            ]
+        }
+    ]
+}
--- a/vendordeps/Phoenix5-5.34.0-beta-4.json
+++ b/vendordeps/Phoenix5-5.34.0-beta-4.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",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json"
+            "offlineFileName": "Phoenix6-frc2025-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,
--- a/vendordeps/Phoenix6-25.0.0-beta-4.json
+++ b/vendordeps/Phoenix6-25.0.0-beta-4.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",
-            "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",
+            "version": "25.2.1",
            "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",
-            "version": "25.0.0-beta-4",
-            "libName": "CTRE_SimProCANrange",
+            "artifactId": "simProPigeon2",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProPigeon2",
            "headerClassifier": "headers",
            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
@@ -371,9 +401,9 @@
        },
        {
            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
-            "version": "25.0.0-beta-4",
-            "libName": "CTRE_SimProPigeon2",
+            "artifactId": "simProCANrange",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProCANrange",
            "headerClassifier": "headers",
            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
--- a/vendordeps/REVLib-2025.0.0-beta-3.json
+++ b/vendordeps/REVLib-2025.0.0-beta-3.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",
--- a/vendordeps/Studica-2025.0.1.json
+++ b/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"
+        }
+    ]
+}
Author	SHA1	Message	Date
Bradley Bickford	a96d96fecb	Merge branch 'main' into kraken_swerve	2025-01-20 20:12:40 -05:00
Bradley Bickford	edb95da65c	Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code	2025-01-20 20:02:53 -05:00
Bradley Bickford	b90056f9ce	Adding basic PathPlanner setup	2025-01-20 20:02:51 -05:00
NoahLacks63	ce7246114f	Added lots of comments, also added a few simple methods as backup	2025-01-21 00:58:38 +00:00
Bradley Bickford	198d105741	Adding AHRS from Studica to the drivetrain so the NavX is assumed to be used, also cleaned up some unused imports	2025-01-20 19:33:33 -05:00
Bradley Bickford	8cbd9bb095	Kraken swerve based on Tyler's original work	2025-01-18 16:04:54 -05:00
Bradley Bickford	4d9aa82520	Adding ArmSysID and some more configuration stuff	2025-01-18 15:00:53 -05:00
Bradley Bickford	c1dddcace5	Resolving some issues related to the use of the CANcoder, and added some missing constants and instantiations for PIDControllers related to the Arm	2025-01-18 12:57:02 -05:00
Bradley Bickford	ecf4916b93	Upgrading to 2025.2.1	2025-01-18 11:12:48 -05:00
NoahLacks63	91cd13f87a	did small amounts of stuff. can't remember	2025-01-18 15:47:18 +00:00
Bradley Bickford	5325920b42	Adjusting moveManipulator so it's not so...rough	2025-01-16 17:59:12 -05:00
NoahLacks63	d1d577f52f	fixed a terrible formatting error	2025-01-16 19:52:38 +00:00
NoahLacks63	11a191440c	added a smooth manipulator translation method	2025-01-16 19:46:20 +00:00
NoahLacks63	f4cfd2874b	began adding elevator and arm methods	2025-01-16 15:26:54 +00:00
NoahLacks63	f391d1540b	One more fix to resolve Noah's merge conflict for him	2025-01-15 02:24:19 +00:00
NoahLacks63	5f111cd4ee	Still resolving merge conflict	2025-01-15 02:21:48 +00:00
NoahLacks63	adb17f9cea	Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code	2025-01-15 02:21:24 +00:00
NoahLacks63	afba8731b3	added a few manual controls for arm and elevator	2025-01-15 02:04:25 +00:00
NoahLacks63	865ba29152	minor subsystem changes	2025-01-13 13:42:48 +00:00