A bunch of random work on Subsystems, lots of work left to do

.vscode/launch.json

@@ -13,7 +13,7 @@
     },
     {
       "type": "wpilib",
-      "name": "WPILib roboRIO Debug",
+      "name": "WPILib Systemcore Debug",
       "request": "launch",
       "desktop": false,
     }

.vscode/settings.json

@@ -37,7 +37,7 @@
     "org.mockito.Mockito.*",
     "org.mockito.ArgumentMatchers.*",
     "org.mockito.Answers.*",
-    "edu.wpi.first.units.Units.*"
+    "org.wpilib.units.Units.*"
   ],
   "java.completion.filteredTypes": [
     "java.awt.*",
@@ -53,9 +53,10 @@
     "javax.swing.*",
     "javax.management.*",
     "javax.smartcardio.*",
-    "edu.wpi.first.math.proto.*",
-    "edu.wpi.first.math.**.proto.*",
-    "edu.wpi.first.math.**.struct.*",
+    "org.wpilib.math.proto.*",
+    "org.wpilib.math.**.proto.*",
+    "org.wpilib.math.**.struct.*",
   ],
+  "java.debug.settings.enableRunDebugCodeLens": false,
   "java.dependency.enableDependencyCheckup": false
 }

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2026",
-    "teamNumber": 2648
+    "projectYear": "2027_alpha7",
+    "teamNumber": 9999
 }

build.gradle

@@ -1,39 +1,45 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.2.1"
+    id "application"
+    id "org.wpilib.GradleRIO" version "2027.0.0-alpha-7"
 }
 
 java {
-    sourceCompatibility = JavaVersion.VERSION_17
-    targetCompatibility = JavaVersion.VERSION_17
+    sourceCompatibility = JavaVersion.VERSION_25
+    targetCompatibility = JavaVersion.VERSION_25
 }
 
-def ROBOT_MAIN_CLASS = "frc.robot.Main"
+def ROBOT_MAIN_CLASS = "first.Main"
 
-// Define my targets (RoboRIO) and artifacts (deployable files)
+// Define my targets (SystemCore) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
 deploy {
     targets {
-        roborio(getTargetTypeClass('RoboRIO')) {
+        systemcore(getTargetTypeClass('SystemCore')) {
             // 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)
+            team = project.wpilib.getTeamNumber()
+            // Use the default systemcore host name. This must be called after setting team
+            // as happens on the line above
+            useDefaultSystemcoreHostName()
+            debug = project.wpilib.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')) {
+                wpilibJava(getArtifactTypeClass('WPILibJavaArtifact')) {
+                    // Set to true to use debug including JNI, which will drastically impact performance.
+                    debugJni = false
                 }
 
                 // Static files artifact
-                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                wpilibStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
-                    directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                    directory = '/home/systemcore/deploy'
+                    deleteOldFiles = false // Change to true to delete files on systemcore that no
                                            // longer exist in deploy directory of this project
                 }
             }
@@ -41,20 +47,14 @@ deploy {
     }
 }
 
-def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+def deployArtifact = deploy.targets.systemcore.artifacts.wpilibJava
 
-// Set to true to use debug for all targets including JNI, which will drastically impact
-// performance.
-wpi.java.debugJni = false
+// Set to true to use debug for simulation including JNI
+wpi.java.runSimWithDebugJni = false
 
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
-task(replayWatch, type: JavaExec) {
-    mainClass = "org.littletonrobotics.junction.ReplayWatch"
-    classpath = sourceSets.main.runtimeClasspath
-}
-
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {
@@ -62,11 +62,11 @@ dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
-    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
-    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+    systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore)
+    systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore)
 
-    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
-    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+    systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore)
+    systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore)
 
     nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
     nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
@@ -78,9 +78,6 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-
-    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
-    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
@@ -92,23 +89,18 @@ test {
 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.
+application.mainClass = ROBOT_MAIN_CLASS
+
+deployArtifact.configureApplication(application)
+wpi.java.configureApplication(application)
+wpi.java.configureTestTasks(test)
+
 jar {
-    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
     from('src') { into 'backup/src' }
     from('vendordeps') { into 'backup/vendordeps' }
     from('build.gradle') { into 'backup' }
-    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
-    duplicatesStrategy = DuplicatesStrategy.INCLUDE
 }
 
-// Configure jar and deploy tasks
-deployArtifact.jarTask = jar
-wpi.java.configureExecutableTasks(jar)
-wpi.java.configureTestTasks(test)
-
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'

gradle/wrapper/gradle-wrapper.properties

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

gradlew

@@ -1,7 +1,7 @@
 #!/bin/sh
 
 #
-# Copyright © 2015-2021 the original authors.
+# Copyright © 2015 the original authors.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -57,7 +57,7 @@
 #       Darwin, MinGW, and NonStop.
 #
 #   (3) This script is generated from the Groovy template
-#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       https://github.com/gradle/gradle/blob/2d6327017519d23b96af35865dc997fcb544fb40/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
 #       within the Gradle project.
 #
 #       You can find Gradle at https://github.com/gradle/gradle/.
@@ -86,8 +86,7 @@ done
 # shellcheck disable=SC2034
 APP_BASE_NAME=${0##*/}
 # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
-' "$PWD" ) || exit
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s\n' "$PWD" ) || exit
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
 MAX_FD=maximum
@@ -115,7 +114,6 @@ case "$( uname )" in                #(
   NONSTOP* )        nonstop=true ;;
 esac
 
-CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
 
 
 # Determine the Java command to use to start the JVM.
@@ -173,7 +171,6 @@ fi
 # For Cygwin or MSYS, switch paths to Windows format before running java
 if "$cygwin" || "$msys" ; then
     APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
-    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
 
     JAVACMD=$( cygpath --unix "$JAVACMD" )
 
@@ -206,15 +203,14 @@ fi
 DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
 
 # Collect all arguments for the java command:
-#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
 #     and any embedded shellness will be escaped.
 #   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
 #     treated as '${Hostname}' itself on the command line.
 
 set -- \
         "-Dorg.gradle.appname=$APP_BASE_NAME" \
-        -classpath "$CLASSPATH" \
-        org.gradle.wrapper.GradleWrapperMain \
+        -jar "$APP_HOME/gradle/wrapper/gradle-wrapper.jar" \
         "$@"
 
 # Stop when "xargs" is not available.

gradlew.bat

@@ -70,11 +70,10 @@ 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 %*
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %*
 
 :end
 @rem End local scope for the variables with windows NT shell

settings.gradle

@@ -2,27 +2,35 @@ import org.gradle.internal.os.OperatingSystem
 
 pluginManagement {
     repositories {
-        mavenLocal()
-        gradlePluginPortal()
-        String frcYear = '2026'
-        File frcHome
-        if (OperatingSystem.current().isWindows()) {
+        String wpilibYear = '2027_alpha7'
+        File wpilibHome
+        def os = OperatingSystem.current()
+        if (os.isWindows()) {
             String publicFolder = System.getenv('PUBLIC')
             if (publicFolder == null) {
                 publicFolder = "C:\\Users\\Public"
             }
             def homeRoot = new File(publicFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
+            wpilibHome = new File(homeRoot, wpilibYear)
+        } else if (os.isLinux()) {
+            String xdgDataHome = System.getenv('XDG_DATA_HOME')
+            if (xdgDataHome == null || xdgDataHome.trim().isEmpty() || !new File(xdgDataHome).isAbsolute()) {
+                xdgDataHome = new File(System.getProperty("user.home"), ".local/share").getPath()
+            }
+            def homeRoot = new File(xdgDataHome, "wpilib")
+            wpilibHome = new File(homeRoot, wpilibYear)
         } else {
             def userFolder = System.getProperty("user.home")
-            def homeRoot = new File(userFolder, "wpilib")
-            frcHome = new File(homeRoot, frcYear)
+            def homeRoot = new File(userFolder, ".wpilib")
+            wpilibHome = new File(homeRoot, wpilibYear)
         }
-        def frcHomeMaven = new File(frcHome, 'maven')
+        def wpilibHomeMaven = new File(wpilibHome, 'maven')
         maven {
-            name = 'frcHome'
-            url = frcHomeMaven
+            name = 'wpilibHome'
+            url = wpilibHomeMaven
         }
+        mavenLocal()
+        gradlePluginPortal()
     }
 }
 

src/main/deploy/example.txt

@@ -1,3 +1,3 @@
-Files placed in this directory will be deployed to the RoboRIO into the
+Files placed in this directory will be deployed to the Systemcore into the
 'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
 to get a proper path relative to the deploy directory.

src/main/deploy/pathplanner/navgrid.json

@@ -1 +0,0 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,tLine truncated

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

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.0,
-        "y": 7.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.0,
-        "y": 7.0
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.0,
-        "y": 6.0
-      },
-      "prevControl": {
-        "x": 3.0,
-        "y": 6.0
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/settings.json

@@ -1,32 +0,0 @@
-{
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
-  "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
-  "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
-  "robotMOI": 6.883,
-  "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
-  "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
-  "wheelCOF": 1.2,
-  "flModuleX": 0.273,
-  "flModuleY": 0.273,
-  "frModuleX": 0.273,
-  "frModuleY": -0.273,
-  "blModuleX": -0.273,
-  "blModuleY": 0.273,
-  "brModuleX": -0.273,
-  "brModuleY": -0.273,
-  "bumperOffsetX": 0.0,
-  "bumperOffsetY": 0.0,
-  "robotFeatures": []
-}

src/main/java/first/Main.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package first;
+
+import org.wpilib.framework.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(first.robot.Robot::new);
+  }
+}

src/main/java/first/robot/Robot.java

@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package first.robot;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.epilogue.Epilogue;
+import org.wpilib.epilogue.Logged;
+import org.wpilib.framework.TimedRobot;
+
+@Logged
+public class Robot extends TimedRobot {
+  private Command autonomousCommand;
+
+  private final RobotContainer robotContainer;
+
+  public Robot() {
+    robotContainer = new RobotContainer();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+    Epilogue.update(this);
+  }
+
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void disabledExit() {}
+
+  @Override
+  public void autonomousInit() {
+    autonomousCommand = robotContainer.getAutonomousCommand();
+
+    if (autonomousCommand != null) {
+      CommandScheduler.getInstance().schedule(autonomousCommand);
+    }
+  }
+
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void autonomousExit() {
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
+    }
+  }
+
+  @Override
+  public void teleopInit() {}
+
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void teleopExit() {}
+
+  @Override
+  public void utilityInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  @Override
+  public void utilityPeriodic() {}
+
+  @Override
+  public void utilityExit() {}
+}

src/main/java/first/robot/RobotContainer.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package first.robot;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.epilogue.Logged;
+
+@Logged
+public class RobotContainer {
+  public RobotContainer() {
+    configureBindings();
+  }
+
+  private void configureBindings() {}
+
+  public Command getAutonomousCommand() {
+    return Commands.print("No autonomous command configured");
+  }
+}

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

@@ -1,19 +1,17 @@
-package frc.robot.constants;
+package first.robot.constants;
 
 import java.io.IOException;
 
 import org.json.simple.parser.ParseException;
+import org.wpilib.math.trajectory.TrapezoidProfile;
 
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.PathConstraints;
 
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-
-// TODO This is all hold over from 2025, does any of it need to change?
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 5;
+    public static final double kMaxSpeedMetersPerSecond = 4;
     public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
@@ -59,4 +57,4 @@ public class AutoConstants {
             kAutoConfigOk = false;
         }
     }
-}
+}

src/main/java/first/robot/constants/ClimberConstants.java

@@ -0,0 +1,68 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ClimberConstants {
+    // TODO Real values
+    public enum ClimberPositions {
+        kStow(0),
+        kClimbOffGround(0),
+        kUp(0);
+
+        private double positionMeters;
+
+        private ClimberPositions(double positionMeters) {
+            this.positionMeters = positionMeters;
+        }
+
+        public double getPositionMeters() {
+            return positionMeters;
+        }
+    }
+
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
+    public static final int kMotorCANID = 0;
+
+    public static final double kConversionFactor = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+
+    public static final boolean kMotorInverted = false;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+        kMotorConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        kMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}

src/main/java/first/robot/constants/CompetitionConstants.java

@@ -0,0 +1,37 @@
+package first.robot.constants;
+
+import org.wpilib.fields.Field;
+import org.wpilib.fields.Fields;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+
+public class CompetitionConstants {
+    public static final Field kField = Fields.FRC_2026_REBUILT_ANDY_MARK.loadField();
+
+    public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
+
+    // TODO Real Values
+    public static final Transform3d kRobotToShooter = new Transform3d();
+
+    public static final Pose2d kBlueHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11), 
+        Units.inchesToMeters(158.84), 
+        Rotation2d.fromDegrees(0)
+    );
+
+    // TODO The origination value produced by the April Tag Field Layout object may
+    // influence what this value should actually be. See AprilTagFieldLayout.getOrigin
+    // For now, the X axis position (forward/backward) is calculated as though the blue
+    // alliance wall right hand side is the originiation point, so, the distance from
+    // the blue alliance wall, to the blue alliance hub center point, plus
+    // the distance between the center of the blue alliance hub and the center of
+    // the red alliance hub
+    public static final Pose2d kRedHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11 + 143.5 * 2),
+        Units.inchesToMeters(158.84),
+        Rotation2d.fromDegrees(0)
+    );
+
+}

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

@@ -1,12 +1,16 @@
-package frc.robot.constants;
+package first.robot.constants;
 
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.util.Units;
+import org.wpilib.hardware.bus.CANPort;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.math.util.Units;
 
 public class DrivetrainConstants {
-    // TODO Hold over from 2025, adjust?
-    public static final double kMaxSpeedMetersPerSecond = 4.125;
+    public static final double kMaxSpeedMetersPerSecond = 4.663;
     public static final double kMaxAngularSpeed = 2 * Math.PI;
 
     public static final double kTrackWidth = Units.inchesToMeters(23.75);
@@ -17,15 +21,17 @@ public class DrivetrainConstants {
     public static final double kRearLeftMagEncoderOffset = 3.761;
     public static final double kRearRightMagEncoderOffset = 2.573;
 
-    public static final int kFrontLeftDrivingCANID = 0;
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
+    public static final int kFrontLeftDrivingCANID = 4;
     public static final int kFrontRightDrivingCANID = 3;
     public static final int kRearLeftDrivingCANID = 1;
     public static final int kRearRightDrivingCANID = 2;
 
-    public static final int kFrontLeftTurningCANID = 8;
-    public static final int kFrontRightTurningCANID = 9;
-    public static final int kRearLeftTurningCANID = 7;
-    public static final int kRearRightTurningCANID = 6;
+    public static final int kFrontLeftTurningCANID = 7; // 8
+    public static final int kFrontRightTurningCANID = 21; //9
+    public static final int kRearLeftTurningCANID = 6; //7
+    public static final int kRearRightTurningCANID = 8; //6
 
     public static final int kFrontLeftAnalogInPort = 3;
     public static final int kFrontRightAnalogInPort = 2;
@@ -35,9 +41,15 @@ public class DrivetrainConstants {
     public static final boolean kGyroReversed = true;
 
     // TODO Hold over from 2025, adjust?
-    public static final double kHeadingP = .1;
+    public static final double kHeadingP = .65;
     public static final double kXTranslationP = .5;
     public static final double kYTranslationP = .5;
+    public static final double kYawPIDTolerance = Units.degreesToRadians(1);
+
+    // TODO How much do we trust gyro and encoders vs vision estimates.
+    // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
+    public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.3, 0.3, 0.3);
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(

src/main/java/first/robot/constants/HoodConstants.java

@@ -0,0 +1,93 @@
+package first.robot.constants;
+
+import java.util.Map;
+
+import org.wpilib.hardware.bus.CANPort;
+import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
+import org.wpilib.math.util.Units;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+
+public class HoodConstants {
+    // TODO Real Values
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+    
+    public static final int kMotorCANID = 12;
+
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
+
+    public static final double kP = 1.75;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0.435;
+    public static final double kV = 0;
+    public static final double kA = 0;
+    public static final double kStartupAngle = 0.0;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
+
+    public static final double kAmpsToTriggerPositionReset = 10;
+
+    // TODO This is just barely longer than the default frame time for output current information
+    // Should this be longer?
+    public static final double kTimeAboveThresholdToReset = .25;
+
+    public static final int kCurrentLimit = 15;
+
+    public static final boolean kInverted = true;
+    public static final boolean kUseInterpolatorForAngle = false;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
+        ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap()
+    );
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kConfig = new SparkMaxConfig();
+
+    static {
+        kConfig
+            .idleMode(kIdleMode)
+            .inverted(kInverted)
+            .smartCurrentLimit(kCurrentLimit);
+        kConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        kConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kS, kV, kA);
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(9.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
+            Double.valueOf(Units.degreesToRadians(12.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
+            Double.valueOf(Units.degreesToRadians(16.25)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
+            Double.valueOf(Units.degreesToRadians(20.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
+            Double.valueOf(Units.degreesToRadians(23.5)));
+    }
+}

src/main/java/first/robot/constants/IntakePivotConstants.java

@@ -0,0 +1,89 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IntakePivotConstants {
+    public enum IntakePivotPosition {
+        kUp(Math.toRadians(116.0)),
+        kDown(Math.toRadians(0.0));
+
+        private double positionRadians;
+        private double positionRotations;
+
+        private IntakePivotPosition(double positionRadians) {
+            this.positionRadians = positionRadians;
+            this.positionRotations = positionRadians / kConversionFactor;
+        }
+
+        public double getPositionRadians() {
+            return positionRadians;
+        }
+
+        public double getPositionRotations() {
+            return positionRotations;
+        }
+    }
+
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
+    public static final int kLeftMotorCANID = 16;
+    public static final int kRightMotorCANID = 9;
+
+    public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
+
+    // Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril
+    public static final double kMaxManualSpeedMultiplier = .3;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 5.26;
+    public static final double kA = 0.05;
+    public static final double kG = 0.25;
+
+    public static final boolean kInvertMotors = false;
+
+    public static final int kCurrentLimit = 30;
+
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+    
+    public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
+
+    static {
+        KLeftMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(false);
+        KLeftMotorConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        KLeftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionWrappingInputRange(0, 2 * Math.PI)
+            .feedForward
+                .svag(kS, kV, kA, kG);
+
+        kRightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(true)
+            ;//.follow(kLeftMotorCANID);
+    }
+}

src/main/java/first/robot/constants/IntakeRollerConstants.java

@@ -1,16 +1,24 @@
-package frc.robot.constants;
+package first.robot.constants;
 
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class IntakeRollerConstants {
-    // TODO Real values
-    public static final int kLeftMotorCANID = 0;
-    public static final int kRightMotorCANID = 0;
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+    
+    public static final int kRightMotorCANID = 20;
+    public static final int kLeftMotorCANID = 1;
 
-    public static final int kCurrentLimit = 30;
+    public static final int kCurrentLimit = 65;
 
-    public static final boolean kInvertMotors = false;
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;
+
+    public static final double kIntakeInSpeed = .9;
+    public static final double kIntakeOutSpeed = 1;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 
@@ -23,12 +31,11 @@ public class IntakeRollerConstants {
         leftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
-
+            .inverted(kInvertLeftMotor);
         rightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
-            .follow(kLeftMotorCANID);
+            .inverted(kInvertRightMotor)
+        ;
     }
 }

src/main/java/first/robot/constants/KrakenMotorConstants.java

@@ -1,5 +1,5 @@
-package frc.robot.constants;
+package first.robot.constants;
 
 public class KrakenMotorConstants {
     public static final double kFreeSpeedRPM = 6000;
-}
+}

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

@@ -1,21 +1,42 @@
-package frc.robot.constants;
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.CANPort;
+import org.wpilib.math.util.Units;
 
 import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 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.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-import edu.wpi.first.math.util.Units;
+import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ModuleConstants {
+    public enum ModuleName {
+        kFrontLeft("FrontLeft"),
+        kFrontRight("FrontRight"),
+        kRearLeft("RearLeft"),
+        kRearRight("RearRight");
+
+        private String loggableName;
+
+        private ModuleName(String loggableName) {
+            this.loggableName = loggableName;
+        }
+
+        public String getLoggableName() {
+            return loggableName;
+        }
+        
+    }
+
     // DRIVING MOTOR CONFIG (Kraken)
-    public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
+    public static final double kDrivingMotorReduction = (50 * 16 * 45)/(14.0 * 28.0 * 15.0);
 
     public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
     public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
@@ -26,16 +47,17 @@ public class ModuleConstants {
     public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS;
     
     // TODO Hold over from 2025, adjust?
-    public static final double kDriveP = .04;
+    public static final double kDriveP = .06;
     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 kClosedLoopRampRate = .01;
 
     // TODO Hold over from 2025, adjust?
-    public static final int kDriveMotorStatorCurrentLimit = 100;
-    public static final int kDriveMotorSupplyCurrentLimit = 65;
+    public static final int kDriveMotorStatorCurrentLimit = 90;
+    public static final int kDriveMotorSupplyCurrentLimit = 40;
 
     // TODO Hold over from 2025, adjust?
     public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
@@ -45,10 +67,12 @@ public class ModuleConstants {
     public static final double kTurningMotorReduction = 150.0/7.0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
     // TODO Adjust? Let over from 2025
-    public static final double kTurnP = 1;
+    public static final double kTurnP = 12;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    public static final double kTurnTolerance = Math.toRadians(0.25);
+
     public static final boolean kIsEncoderInverted = false;
 
     // TODO How sensitive is too sensitive?
@@ -67,6 +91,7 @@ public class ModuleConstants {
     public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
     public static final AudioConfigs kAudioConfig = new AudioConfigs();
     public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
+    public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs();
 
     static {
         kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
@@ -88,19 +113,27 @@ public class ModuleConstants {
         kDriveSlot0Config.kV = kDriveV;
         kDriveSlot0Config.kA = kDriveA;
 
+        kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
+
         turningConfig
             .idleMode(kTurnIdleMode)
             .smartCurrentLimit(kTurnMotorCurrentLimit)
             .inverted(true);
         turningConfig.encoder
-            //.inverted(true)
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
             .positionConversionFactor(kTurningFactor)
             .velocityConversionFactor(kTurningFactor / 60.0);
         turningConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
             .positionWrappingEnabled(true)
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
             .positionWrappingInputRange(0, 2 * Math.PI);
             
     }

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

@@ -1,6 +1,6 @@
-package frc.robot.constants;
+package first.robot.constants;
 
-import edu.wpi.first.wpilibj.util.Color;
+import org.wpilib.util.Color;
 
 public class OIConstants {
     public static final int kDriverControllerPort = 0;
@@ -15,15 +15,15 @@ public class OIConstants {
     public static final String kCurrentActiveHub = "Alliance Hub Currently Active";
 
     public static final String[] kRedBlueDisplay = {
-        Color.kRed.toHexString(),
-        Color.kBlue.toHexString()
+        Color.RED.toHexString(),
+        Color.BLUE.toHexString()
     };
 
     public static final String[] kRedDisplay = {
-        Color.kRed.toHexString()
+        Color.RED.toHexString()
     };
 
     public static final String[] kBlueDisplay = {
-        Color.kBlue.toHexString()
+        Color.BLUE.toHexString()
     };
 }

src/main/java/first/robot/constants/PhotonConstants.java

@@ -0,0 +1,48 @@
+package first.robot.constants;
+
+import java.util.List;
+
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+
+import first.robot.utilities.PhotonVisionConfig;
+
+public class PhotonConstants {
+    public static final String kCamera1Name = "CameraPV1";
+    public static final String kCamera2Name = "CameraPV2";
+
+    // TODO Need actual values for all of this
+    public static final Transform3d kCamera1RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-8.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(30.0)
+        )
+    );
+    public static final Transform3d kCamera2RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-10.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0.0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(-10.0)
+        )
+    );
+
+    public static final double kCamera1HeightMeters = 0;
+    public static final double kCamera1PitchRadians = 0;
+    public static final double kCamera2HeightMeters = 0;
+    public static final double kCamera2PitchRadians = 0;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final List<PhotonVisionConfig> configs = List.of(
+        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
+    );
+}

src/main/java/first/robot/constants/RollerIndexerConstants.java

@@ -0,0 +1,28 @@
+package first.robot.constants;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class RollerIndexerConstants {
+    public static final int kMotor1CANID = 50;
+    public static final int kMotor2CANID = 4;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final double kMotorSpeed = 1;
+
+    public static final boolean kMotorInverted = true;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+    
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+    }
+}

src/main/java/first/robot/constants/ShooterConstants.java

@@ -0,0 +1,112 @@
+package first.robot.constants;
+
+import org.wpilib.math.util.Units;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ShooterConstants {
+    public enum ShooterSpeeds {
+        kHubSpeed(3000.0),
+        kFeedSpeed(5000.0),
+        kIdleSpeed(750.0);
+
+        private double speedMPS;
+        private double speedRPM;
+
+        private ShooterSpeeds(double speedRPM) {
+            this.speedMPS = speedRPM * kWheelDiameter*Math.PI;
+            this.speedRPM = speedRPM;
+        }
+
+        public double getSpeedMPS() {
+            return speedMPS * kWheelDiameter*Math.PI;
+        }
+
+        public double getSpeedRPM(){
+            return speedRPM;
+        }
+    }
+
+    // TODO Conversion factor?
+
+    public static final double kWheelDiameter = Units.inchesToMeters(4);
+
+    // TODO Real values
+    public static final int kLeftShooterMotorCANID = 2;
+    public static final int kRightShooterMotorCANID = 5;
+
+    public static final boolean kLeftShooterMotorInverted = true;
+    public static final boolean kRightShooterMotorInverted = false;
+
+    public static final double kLeftP = 0.75;//0.01;//0.001;
+    public static final double kLeftI = 0;
+    public static final double kLeftD = 0;//0.1;//1.8;
+    public static final double kLeftS = 0;
+    public static final double kLeftV = 0.00129;
+    public static final double kLeftA = 0;
+
+    public static final double kRightP = 0.75;//0.001;//0.001;
+    public static final double kRightI = 0;
+    public static final double kRightD = 0;//0.1;
+    public static final double kRightS = 0;
+    public static final double kRightV = 0.00125;
+    public static final double kRightA = 0;
+
+    public static final double kMaxManualSpeedMultiplier = 1;
+
+    public static final double kShooterHeightMeters = 0;
+
+    // TODO Is this value sane?
+    public static final int kCurrentLimit = 60;
+
+    public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
+
+    static {
+        kLeftMotorConfig
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kLeftShooterMotorInverted);
+        kLeftMotorConfig.absoluteEncoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(1)
+            .velocityConversionFactor(60)
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+        kLeftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
+
+        kRightMotorConfig
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kRightShooterMotorInverted);
+        kRightMotorConfig.absoluteEncoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(1)
+            .velocityConversionFactor(60)
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+            .inverted(true);
+        kRightMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kRightP, kRightI, kRightD)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
+    }
+}

src/main/java/first/robot/constants/SpindexerConstants.java

@@ -1,25 +1,28 @@
-package frc.robot.constants;
+package first.robot.constants;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class SpindexerConstants {
     // TODO Real values
     public static final int kSpindexerMotorCANID = 0;
-    public static final int kFeederMotorCANID = 0;
+    public static final int kFeederMotorCANID = 4;
 
-    public static final int kSpindexerStatorCurrentLimit = 80;
-    public static final int kSpindexerSupplyCurrentLimit = 30;
+    public static final int kSpindexerStatorCurrentLimit = 95;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
     public static final int kFeederCurrentLimit = 30;
 
+    public static final double kSpindexerSpeed = 1;
+    public static final double kFeederSpeed = 1;
+
     public static final boolean kFeederMotorInverted = false;
 
     public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
-    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake;
+    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
 
     public static final IdleMode kFeederIdleMode = IdleMode.kBrake;
 

src/main/java/first/robot/constants/SystemCoreConstants.java

@@ -0,0 +1,7 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
+
+public class SystemCoreConstants {
+    public static final MountOrientation kSystemCoreMountOrientation = MountOrientation.FLAT;
+}

src/main/java/first/robot/interfaces/IIndexer.java

@@ -0,0 +1,13 @@
+package first.robot.interfaces;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+
+public interface IIndexer {
+    public Command spinToShooter();
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM);
+    public Command spinToIntake();
+    public Command stop();
+    public Command instantaneousStop();
+}

src/main/java/first/robot/subsystems/Climber.java

@@ -0,0 +1,88 @@
+package first.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.ClimberConstants;
+import first.robot.constants.ClimberConstants.ClimberPositions;
+
+@Logged
+public class Climber extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private ClimberPositions targetPosition;
+
+    public Climber() {
+        motor = new SparkMax(
+            ClimberConstants.kCANPort,
+            ClimberConstants.kMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        motor.configure(
+            ClimberConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        encoder = motor.getEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        targetPosition = null;
+    }
+
+    public double getTargetPositionMeters() {
+        if (targetPosition == null) {
+            return -1;
+        } else {
+            return targetPosition.getPositionMeters();
+        }
+    }
+
+    public double getCurrentPositionMeters() {
+        return encoder.getPosition().get();
+    }
+
+    public boolean isAtSetpoint() {
+        return controller.isAtSetpoint().get();
+    }
+
+    public Command maintainPosition(ClimberPositions position) {
+        return run(() -> {
+            targetPosition = position;
+
+            controller.setSetpoint(
+                position.getPositionMeters(), 
+                ControlType.kPosition
+            );
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            targetPosition = null;
+
+            motor.setThrottle(speed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+}

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

@@ -0,0 +1,444 @@
+package first.robot.subsystems;
+
+import java.util.List;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.PrintCommand;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.hardware.imu.OnboardIMU;
+import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+import org.wpilib.math.util.MathUtil;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import first.robot.constants.AutoConstants;
+import first.robot.constants.DrivetrainConstants;
+import first.robot.constants.ModuleConstants.ModuleName;
+import first.robot.constants.OIConstants;
+import first.robot.constants.SystemCoreConstants;
+import first.robot.utilities.SwerveModule;
+import first.robot.utilities.Utilities;
+import first.robot.utilities.VisualPose;
+
+public class Drivetrain extends SubsystemBase {
+    private SwerveModule frontLeft;
+    private SwerveModule frontRight;
+    private SwerveModule rearLeft;
+    private SwerveModule rearRight;
+
+    private OnboardIMU imu;
+
+    private SwerveDrivePoseEstimator estimator;
+
+    private PIDController yawRotationController;
+
+    public Drivetrain(Pose2d startupPose) {
+        frontLeft = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kFrontLeft, 
+            DrivetrainConstants.kFrontLeftDrivingCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftAnalogInPort, 
+            DrivetrainConstants.kFrontLeftMagEncoderOffset
+        );
+
+        frontRight = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kFrontRight, 
+            DrivetrainConstants.kFrontRightDrivingCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightAnalogInPort, 
+            DrivetrainConstants.kFrontRightMagEncoderOffset
+        );
+
+        rearLeft = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kRearLeft, 
+            DrivetrainConstants.kRearLeftDrivingCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftAnalogInPort, 
+            DrivetrainConstants.kRearLeftMagEncoderOffset
+        );
+
+        rearRight = new SwerveModule(
+            DrivetrainConstants.kCANPort,
+            ModuleName.kRearRight, 
+            DrivetrainConstants.kRearRightDrivingCANID, 
+            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightAnalogInPort, 
+            DrivetrainConstants.kRearRightMagEncoderOffset
+        );
+
+        imu = new OnboardIMU(SystemCoreConstants.kSystemCoreMountOrientation);
+
+        yawRotationController = new PIDController(
+            DrivetrainConstants.kHeadingP, 
+            0, 
+            0
+        );
+        yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
+        yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance);
+
+        // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
+        estimator = new SwerveDrivePoseEstimator(
+            DrivetrainConstants.kDriveKinematics, 
+            Rotation2d.fromDegrees(getGyroValue()), 
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            startupPose != null ? startupPose : new Pose2d(),
+            DrivetrainConstants.kSensorFusionOdometryStdDevs,
+            DrivetrainConstants.kVisionOdometryStdDevs
+        );
+
+        if(AutoConstants.kAutoConfigOk) {
+            AutoBuilder.configure(
+                this::getPose, 
+                this::resetOdometry, 
+                this::getCurrentChassisSpeeds, 
+                (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), 
+                AutoConstants.kPPDriveController, 
+                AutoConstants.kRobotConfig,
+                () -> {
+                    Optional<Alliance> alliance = MatchState.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == Alliance.RED;
+                        }
+                        return false;
+                }, 
+                this
+            );
+        }
+    }
+
+    @Override
+    public void periodic() {
+        estimator.update(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            }
+        );
+
+        frontLeft.periodic();
+        frontRight.periodic();
+        rearLeft.periodic();
+        rearRight.periodic();
+
+        Logger.recordOutput("Drivetrain/Pose", getPose());
+        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
+        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
+        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
+    }
+
+    /**
+     * Can be used to run an individual module on the drive base a static speed while maintaining a static angle.
+     * 
+     * Good for diagnosing issues with swerve module configuration. Essentially useless otherwise.
+     * 
+     * @param name The ModuleName enumeration that indicates which module you want to control
+     * @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at
+     * @param staticAngleDegrees The static angle in degrees that you want the wheel to face
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) {
+        SwerveModule module = List.of(
+            frontLeft,
+            frontRight,
+            rearLeft,
+            rearRight
+        ).stream()
+        .filter((m) -> m.getModuleName() == name)
+        .findFirst()
+        .get();
+
+        return run(() -> {
+            module.setDesiredState(new SwerveModuleVelocity(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)
+            ));
+        });
+    }
+
+    public Command disableOutputs() {
+        return run(() -> {
+            frontLeft.disableOutput();
+            frontRight.disableOutput();
+            rearLeft.disableOutput();
+            rearRight.disableOutput();
+        });  
+    }
+
+    /**
+     * Rotates the robot to a face a given Pose2d position on the field
+     * 
+     * Note that this Command does not provide a means of timeout. If you are
+     * using this in an auto context, this Command should be decorated with 
+     * withTimeout(<some_value>). Otherwise, you will be waiting for the PID
+     * Controller doing the work to report that it is at the desired setpoint. 
+     * 
+     * @param targetPose The Pose2d object to rotate the robot towards
+     * @param rotate180 When false, the front of the robot faces the specified pose, when true
+     *   the back of the robot faces the specified pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
+            .until(yawRotationController::atSetpoint);
+    }
+
+    /**
+     * Locks the robots rotation to face the Alliance Hub on the field. 
+     * 
+     * This method is innately aware of which hub to face based on the assigned alliance color.
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the center of the alliance hub. This
+     * method assumes that the robots estimated pose is reasonably accurate. 
+     * 
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the hub, when true, the back
+     *   of the robot faces the hub
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return lockRotationToSuppliedPose(
+            Utilities::getHubPose, 
+            xSpeed, 
+            ySpeed, 
+            rotate180
+        );
+    }
+
+    /**
+     * Locks the robots rotation to face a particular pose on the field
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the Pose2d object being supplied. This 
+     * method assumes that the robots estimated pose is reasonably accurate.
+     * 
+     * @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
+     *   of the robot faces the supplied pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> {
+                    Pose2d faceTowards = poseSupplier.get();
+
+                    Rotation2d targetRotation = new Rotation2d(
+                        faceTowards.getX() - getPose().getX(), 
+                        faceTowards.getY() - getPose().getY()
+                    );
+
+                    if(rotate180) {
+                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
+                    }
+
+                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
+                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
+
+                    double outputPower = -yawRotationController.calculate(
+                        getHeading().getRadians(),
+                        targetRotation.getRadians()
+                    );
+
+                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
+
+                    return outputPower;
+                }, 
+                () -> true
+            )
+        );
+    }
+
+    /**
+     * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from
+     * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced
+     * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in
+     * other terms, to center the provided tag in the camera's image frame.
+     * 
+     * @param yaw The "yaw" of the tag source relative to the center of the image frame
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
+                () -> true
+            )
+        );
+    }
+
+    public Command drivePathPlannerPath(PathPlannerPath path) {
+        if(AutoConstants.kAutoConfigOk) {
+            return AutoBuilder.followPath(path);
+        } else {
+            return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled");
+        }
+        
+    }
+
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
+        // TODO Specific Alliance code?
+        return run(() -> {
+            drive(
+                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
+                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
+                fieldRelative.getAsBoolean()
+            );
+        });
+    }
+
+    public Command setX() {
+        return run(() -> {
+            frontLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45)));
+            frontRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45)));
+            rearLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45)));
+            rearRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45)));
+        });
+    }
+
+    public Command zeroHeading() {
+        return run(() -> {
+            imu.resetYaw();;
+            estimator.resetRotation(new Rotation2d(0));
+        });
+    }
+
+    public void consumeVisualPose(VisualPose pose) {
+        if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) {
+            return;
+        }
+        
+        estimator.addVisionMeasurement(
+            pose.visualPose(), 
+            pose.timestamp()
+        );
+    }
+
+    public void resetEncoders() {
+        frontLeft.resetEncoders();
+        frontRight.resetEncoders();
+        rearLeft.resetEncoders();
+        rearRight.resetEncoders();
+    }
+
+    public void resetOdometry(Pose2d pose) {
+        estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            pose
+        );
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+        double xSpeedDelivered = 0;
+        double ySpeedDelivered = 0;
+
+        if(p != 0){
+            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        }else{
+            xSpeedDelivered = 0;
+            ySpeedDelivered = 0;
+        }
+
+        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
+
+        ChassisVelocities velocities = new ChassisVelocities(xSpeedDelivered, ySpeedDelivered, rotationDelivered);
+
+        SwerveModuleVelocity[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities(
+            fieldRelative ?
+                velocities.toFieldRelative(Rotation2d.fromDegrees(getGyroValue())) : velocities 
+        );
+
+        setModuleStates(swerveModuleStates);
+    }
+
+    public void driveWithChassisSpeeds(ChassisVelocities speeds) {
+        ChassisVelocities discreteSpeeds = speeds.discretize(0.2);
+        SwerveModuleVelocity[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities(discreteSpeeds);
+
+        setModuleStates(newStates);
+    }
+
+    public void setModuleStates(SwerveModuleVelocity[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+        frontLeft.setDesiredState(desiredStates[0]);
+        frontRight.setDesiredState(desiredStates[1]);
+        rearLeft.setDesiredState(desiredStates[2]);
+        rearRight.setDesiredState(desiredStates[3]);
+    }
+
+    public ChassisVelocities getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisVelocities(
+            frontLeft.getState(),
+            frontRight.getState(),
+            rearLeft.getState(),
+            rearRight.getState()
+        );
+    }
+
+    public Pose2d getPose() {
+        return estimator.getEstimatedPosition();
+    }
+
+    public double getGyroValue() {
+        return imu.getYawRadians() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+    }
+
+    public Rotation2d getHeading() {
+        return estimator.getEstimatedPosition().getRotation();
+    }
+
+    public double getHeadingDegrees() {
+        return estimator.getEstimatedPosition().getRotation().getDegrees();
+    }
+}
+

src/main/java/first/robot/subsystems/Hood.java

@@ -0,0 +1,183 @@
+package first.robot.subsystems;
+
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
+import org.wpilib.math.util.MathUtil;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.HoodConstants;
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+import first.robot.utilities.Utilities;
+
+public class Hood extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    //private Trigger resetTrigger;
+    //private Trigger timerTrigger;
+
+    //private Timer resetTimer;
+
+    private double currentTargetDegrees;
+
+    public Hood() {
+        motor = new SparkMax(
+            HoodConstants.kCANPort, 
+            HoodConstants.kMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        motor.configure(
+            HoodConstants.kConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );   
+
+        encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
+
+        controller = motor.getClosedLoopController();
+
+        /*resetTimer = new Timer();
+        resetTimer.reset();
+
+        resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
+        resetTrigger.onTrue(new InstantCommand(resetTimer::start));
+        resetTrigger.onFalse(new InstantCommand(() -> {
+            resetTimer.stop();
+            resetTimer.reset();
+        }));
+
+        timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset));
+        timerTrigger.onTrue(new InstantCommand(() -> {
+            encoder.setPosition(0);
+            resetTimer.reset();
+        }));*/
+
+        currentTargetDegrees = HoodConstants.kStartupAngle;
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber(
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        SmartDashboard.putBoolean(
+            "HoodAtSetpoint", 
+            controller.isAtSetpoint()
+        );
+        
+        Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
+        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
+    }
+
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
+    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
+        return run(() -> {
+            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
+        });
+    }
+
+    /**
+     * An automated form of resetting the hood position sensing.
+     * 
+     * Run down at the full manual speed (note that this is affected by the 
+     * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true
+     * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) 
+     * for reset for the amount of specified by kTimeAboveThresholdToReset). Once
+     * that returns true, the motor is stopped until the timer trigger switches to false
+     * (i.e. it has reset the position automatically, because that's how it's configured, 
+     * and resets the timer to 0, which makes the timer trigger false)
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezero() {
+        return manualSpeed(() -> -1)
+            .until(timerTrigger)
+            .andThen(
+                stop().until(timerTrigger.negate())
+        );
+    }
+
+    /**
+     * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers
+     * to reset the hood position to zero. Note that this method doesn't have any time limiting
+     * factor to it, as soon as the current goes above the threshold specified by 
+     * kAmpsToTriggerPositionReset the encoder position will be set to zero
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezeroNoTimer() {
+        return manualSpeed(() -> -1)
+            .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
+            .andThen(new InstantCommand(() -> encoder.setPosition(0)));
+    }*/
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        currentTargetDegrees = 0;
+
+        return run(() -> {
+            motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public double getTargetDegrees() {
+        return currentTargetDegrees;
+    }
+}

src/main/java/first/robot/subsystems/IntakePivot.java

@@ -0,0 +1,125 @@
+package first.robot.subsystems;
+
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.command2.SubsystemBase;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.IntakePivotConstants;
+import first.robot.constants.IntakePivotConstants.IntakePivotPosition;
+
+public class IntakePivot extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private IntakePivotPosition currentTargetPosition;
+
+    public IntakePivot() {
+        leftMotor = new SparkMax(
+            IntakePivotConstants.kCANPort,
+            IntakePivotConstants.kLeftMotorCANID, 
+            MotorType.kBrushless
+        );
+        rightMotor = new SparkMax(
+            IntakePivotConstants.kCANPort,
+            IntakePivotConstants.kRightMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        leftMotor.configure(
+            IntakePivotConstants.KLeftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakePivotConstants.kRightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        controller = leftMotor.getClosedLoopController();
+
+        encoder = leftMotor.getEncoder();
+        encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians());
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput(
+            "IntakePivot/TargetPosition", 
+            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
+        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
+        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command maintainPosition(IntakePivotPosition position) {
+        return run(() -> {
+            currentTargetPosition = position;
+
+            if(currentTargetPosition == null) {
+                leftMotor.disable();
+            } else {
+                controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
+            }
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            currentTargetPosition = null;
+            
+            leftMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+            rightMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(double time){
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time),
+            manualSpeed(() -> 0.75).withTimeout(time)
+        );
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(DoubleSupplier time) {
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
+            manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
+        );
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public Optional<IntakePivotPosition> getCurrentTargetPosition() {
+        return Optional.ofNullable(currentTargetPosition);
+    }
+}
+

src/main/java/first/robot/subsystems/IntakeRoller.java

@@ -0,0 +1,64 @@
+package first.robot.subsystems;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.IntakeRollerConstants;
+
+public class IntakeRoller extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    public IntakeRoller() {
+        leftMotor = new SparkMax(
+            IntakeRollerConstants.kCANPort,
+            IntakeRollerConstants.kLeftMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        rightMotor = new SparkMax(
+            IntakeRollerConstants.kCANPort,
+            IntakeRollerConstants.kRightMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        leftMotor.configure(
+            IntakeRollerConstants.leftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakeRollerConstants.rightMotorConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
+    }
+
+    public Command runIn() {
+        return run(() -> {
+            leftMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed);
+            rightMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed);
+        });
+    }
+
+    public Command runOut() {
+        return run(() -> {
+            leftMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed);
+            rightMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            leftMotor.setThrottle(0);
+            rightMotor.setThrottle(0);
+        });
+    }
+    
+}

src/main/java/first/robot/subsystems/PhotonVision.java

@@ -0,0 +1,204 @@
+package first.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+import java.util.function.Consumer;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Transform3d;
+
+import first.robot.constants.CompetitionConstants;
+import first.robot.constants.PhotonConstants;
+import first.robot.utilities.PhotonVisionConfig;
+import first.robot.utilities.VisualPose;
+
+/**
+ * This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as
+ * a "resource", that is, something that is not inherently a physical mechanism to be controlled.
+ * 
+ * A "resource" in this instance should be thought of as something that can be safely shared
+ * by other Subsystems generally without collision if more that one Subsystem requires the 
+ * "resource" at any given time.
+ * 
+ * Resources should <i>NOT</i> produce Commands, they should not have a default Command.
+ * Resources do not have behaviors, and because Commands are in of themselves behaviors,
+ * this class should not have Commands. 
+ * 
+ * Part of the thinking behind creating the PhotonVision components this way is to rely
+ * on the CommandScheduler to call periodic. If this weren't the case, some other subsystem
+ * would have to manage calling for periodic updates, while still sharing the resource with 
+ * other subsystems <i>somehow</i>. 
+ * 
+ * This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs"
+ * List in the PhotonConstants file, you change what is set up internally in this class. 
+ * 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras,
+ * 2 estimators, etc. etc. 
+ */
+public class PhotonVision extends SubsystemBase {
+    private PhotonCamera[] cameras;
+    private PhotonPoseEstimator[] estimators;
+    private List<PhotonPipelineResult> latestResults;
+
+    private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;
+
+    public PhotonVision() {
+        cameras = new PhotonCamera[PhotonConstants.configs.size()];
+        estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
+        latestResults = new ArrayList<PhotonPipelineResult>();
+
+        for(int i = 0; i < PhotonConstants.configs.size(); i++) {
+            cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName());
+            estimators[i] = new PhotonPoseEstimator(
+                CompetitionConstants.kField, 
+                PhotonConstants.configs.get(i).robotToCamera()
+            );
+            latestResults.add(null);
+        }
+
+        poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < cameras.length; i++) {
+            List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
+
+            if(!results.isEmpty()) {
+                latestResults.set(i, results.get(results.size() - 1));
+
+                Optional<EstimatedRobotPose> pose = estimators[i].estimateCoprocMultiTagPose(latestResults.get(i));
+
+                if(!pose.isEmpty()) {
+                    VisualPose visualPose = new VisualPose(
+                        cameras[i].getName(),
+                        pose.get().estimatedPose.toPose2d(), 
+                        pose.get().timestampSeconds
+                    );
+
+                    for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                        consumer.accept(visualPose);
+                    }
+                }
+            }
+        }
+    }
+
+    /**
+     * Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return Optional.empty().
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that 
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still return
+     * some sort of pose. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An Optional object containing a Pose3d object, or Optional.empty() if
+     *   the tag is not present anywhere in the robots field of view. 
+     */
+    public Optional<Pose3d> getBestPoseForTag(int tagID) {
+        PhotonVisionConfig config = null;
+        Transform3d bestCameraToTarget = null;
+        float bestConfidence = -1;
+
+        for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) {
+            if(latestResults.get(cameraIndex) == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) {
+                    continue;
+                }
+
+                config = PhotonConstants.configs.get(cameraIndex);
+                bestCameraToTarget = target.bestCameraToTarget;
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestCameraToTarget == null) {
+            return Optional.empty();
+        }
+
+        // This is based on what PhotonVision does for multitag Pose estimation
+        // See PhotonPoseEstimator.multiTagOnCoprocStrategy
+        // TODO This doesn't currently account for the offset of the tag relative to say the hub
+        // unclear if that offset amount will be important or not
+        return Optional.of(Pose3d.ZERO
+            .plus(bestCameraToTarget.inverse())
+            .relativeTo(CompetitionConstants.kField.getOrigin())
+            .plus(config.robotToCamera().inverse()));
+    }
+
+    /**
+     * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return OptionalDouble.empty()
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still
+     * return some sort of yaw value.
+     * 
+     * Note that the yaw value here is the yaw of the observed tag relative to the center
+     * of the cameras image frame. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An OptionalDouble object containing a Double representing the described yaw
+     *   of the AprilTag specified, or OptionalDouble.empty() if the tag is not present
+     *   anywhere in the robots field of view
+     */
+    public OptionalDouble getBestYawForTag(int tagID) {
+        double bestTagYaw = -1;
+        float bestConfidence = -1;
+
+        for(PhotonPipelineResult result: latestResults) {
+            if(result == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: result.getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) {
+                    continue;
+                }
+
+                bestTagYaw = target.getYaw();
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestConfidence == -1) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(bestTagYaw);
+    }
+
+    /**
+     * Add a Consumer of VisualPose records to the PhotonVision resource. 
+     * 
+     * Each consumer will receive a VisualPose object when any camera produces a new
+     * VisualPose. 
+     * 
+     * The number of Poses produced in a given 20ms cycle is the same number as how many
+     * cameras there are on the robot, assuming those cameras see enough tags to generate a pose, 
+     * as currently all cameras configuration will generate a Pose2d
+     * 
+     * @param consumer The lambda, functional reference, or Consumer implementing object 
+     *  that will consume Poses produced by the PhotonVision resource.
+     */
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
+    }
+}
+

src/main/java/first/robot/utilities/Elastic.java

@@ -1,28 +1,27 @@
-// Copyright (c) 2023-2026 Gold87 and other Elastic contributors
-// This software can be modified and/or shared under the terms
-// defined by the Elastic license:
-// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
+package first.robot.utilities;
 
-package frc.robot.utilities;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.networktables.StringPublisher;
+import org.wpilib.networktables.StringTopic;
 
-import com.fasterxml.jackson.annotation.JsonProperty;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.PubSubOption;
-import edu.wpi.first.networktables.StringPublisher;
-import edu.wpi.first.networktables.StringTopic;
+import io.avaje.json.JsonException;
+import io.avaje.jsonb.Json;
+import io.avaje.jsonb.JsonType;
+import io.avaje.jsonb.Jsonb;
 
 public final class Elastic {
   private static final StringTopic notificationTopic =
       NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
   private static final StringPublisher notificationPublisher =
-      notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
+      notificationTopic.publish(PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES);
   private static final StringTopic selectedTabTopic =
       NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
   private static final StringPublisher selectedTabPublisher =
-      selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
-  private static final ObjectMapper objectMapper = new ObjectMapper();
+      selectedTabTopic.publish(PubSubOption.KEEP_DUPLICATES);
+  private static final Jsonb jsonb = Jsonb.builder().build();
+  private static final JsonType<Notification> notificationJsonType =
+      jsonb.type(Notification.class);
 
   /**
    * Represents the possible levels of notifications for the Elastic dashboard. These levels are
@@ -45,8 +44,8 @@ public final class Elastic {
    */
   public static void sendNotification(Notification notification) {
     try {
-      notificationPublisher.set(objectMapper.writeValueAsString(notification));
-    } catch (JsonProcessingException e) {
+      notificationPublisher.set(notificationJsonType.toJson(notification));
+    } catch (JsonException e) {
       e.printStackTrace();
     }
   }
@@ -79,23 +78,24 @@ public final class Elastic {
    * properties such as level, title, description, display time, and dimensions to control how the
    * notification is displayed on the dashboard.
    */
+  @Json
   public static class Notification {
-    @JsonProperty("level")
+    @Json.Property("level")
     private NotificationLevel level;
 
-    @JsonProperty("title")
+    @Json.Property("title")
     private String title;
 
-    @JsonProperty("description")
+    @Json.Property("description")
     private String description;
 
-    @JsonProperty("displayTime")
+    @Json.Property("displayTime")
     private int displayTimeMillis;
 
-    @JsonProperty("width")
+    @Json.Property("width")
     private double width;
 
-    @JsonProperty("height")
+    @Json.Property("height")
     private double height;
 
     /**

src/main/java/first/robot/utilities/PhotonVisionConfig.java

@@ -0,0 +1,10 @@
+package first.robot.utilities;
+
+import org.wpilib.math.geometry.Transform3d;
+
+public record PhotonVisionConfig (
+    String cameraName,
+    Transform3d robotToCamera,
+    double cameraHeightMeters,
+    double cameraPitchRadians
+) {}

src/main/java/first/robot/utilities/SparkMAXTester.java

@@ -0,0 +1,38 @@
+package first.robot.utilities;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+/**
+ * A simple subsystem that can be used to test a single SparkMax and associated NEO motor
+ */
+public class SparkMAXTester extends SubsystemBase {
+    private SparkMax spark;
+
+    /**
+     * Constructor
+     * 
+     * @param deviceID The CAN ID of the SparkMAX that needs testing
+     */
+    public SparkMAXTester(CANPort canPort, int deviceID) {
+        spark = new SparkMax(canPort, deviceID, MotorType.kBrushless);
+    }
+
+    /**
+     * Sets the speed of the motor
+     * 
+     * @param speed A method or lambda which returns a double between -1 and 1
+     * @return A Command object that runs indefinitely to control motor speed
+     */
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            spark.setThrottle(speed.getAsDouble());
+        });
+    }
+}

src/main/java/first/robot/utilities/SwerveModule.java

@@ -1,22 +1,24 @@
-package frc.robot.utilities;
+package first.robot.utilities;
 
-import org.littletonrobotics.junction.Logger;
+import org.wpilib.hardware.bus.CANPort;
+import org.wpilib.hardware.rotation.AnalogEncoder;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.AnalogEncoder;
-import frc.robot.constants.ModuleConstants;
+import first.robot.constants.ModuleConstants;
+import first.robot.constants.ModuleConstants.ModuleName;
 
 /*
  * This thread
@@ -30,6 +32,8 @@ import frc.robot.constants.ModuleConstants;
  * the controller closed loop controller. 
  */
 public class SwerveModule {
+    private ModuleName moduleName;
+
     private TalonFX drive;
     private SparkMax turning;
 
@@ -41,10 +45,8 @@ public class SwerveModule {
 
     private VelocityVoltage driveVelocityRequest;
 
-    private String moduleName;
-
-    private SwerveModuleState lastTargetState;
-    private SwerveModuleState lastTargetStateOptimized;
+    private SwerveModuleVelocity lastTargetState;
+    private SwerveModuleVelocity lastTargetStateOptimized;
 
     private boolean isAbsoluteEncoderDisabled;
     private boolean turningEncoderAutoRezeroEnabled;
@@ -55,32 +57,35 @@ public class SwerveModule {
      * This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left)
      * before booting up the robot.
      * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
      * @param moduleName The module name, Front Left, Front Right, etc.
      * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
      * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID) {
-        this(moduleName, drivingCANID, turningCANID, -1, -1);
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID) {
+        this(canPort, moduleName, drivingCANID, turningCANID, -1, -1);
     }
 
     /**
      * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder
      * from the absolute encoder. 
      * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
      * @param moduleName The module name, Front Left, Front Right, etc.
      * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
      * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
      * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
      * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
-        this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        this(canPort, moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
     }
 
     /**
      * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing
      * when the turning motor is not moving
      * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
      * @param moduleName The module name, Front Left, Front Right, etc.
      * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
      * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
@@ -88,12 +93,12 @@ public class SwerveModule {
      * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
      * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, 
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, 
         int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
         isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
 
-        drive = new TalonFX(drivingCANID);
-        turning = new SparkMax(turningCANID, MotorType.kBrushless);
+        drive = new TalonFX(drivingCANID, new CANBus(canPort));
+        turning = new SparkMax(canPort, turningCANID, MotorType.kBrushless);
 
         turningRelativeEncoder = turning.getEncoder();
 
@@ -108,6 +113,7 @@ public class SwerveModule {
         drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
         drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
         drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);
 
         turning.configure(
             ModuleConstants.turningConfig,
@@ -130,21 +136,23 @@ public class SwerveModule {
 
         this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
 
-        this.moduleName = "Drivetrain/Modules/" + moduleName;
+        this.moduleName = moduleName;
     }
 
     public void periodic() {
         
         if(!isAbsoluteEncoderDisabled) {
-            Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+            Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         }
         
-        Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
-        Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
-        Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
-        Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
-        Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
 
+        // TODO Re-enable this? Was turned off when there was drivetrain issues
+        // Now that there aren't, do we try this again?
         /*
         if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
             if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
@@ -153,8 +161,12 @@ public class SwerveModule {
         }*/
     }
 
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(
+    public ModuleName getModuleName() {
+        return moduleName;
+    }
+
+    public SwerveModuleVelocity getState() {
+        return new SwerveModuleVelocity(
             drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
             new Rotation2d(getTurningEncoderPosition())
         );
@@ -172,20 +184,18 @@ public class SwerveModule {
         turning.disable();
     }
 
-    public void setDesiredState(SwerveModuleState desiredState) {
-        lastTargetState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
-        // TODO is this really necessary, the offset is managed by the Absolute Encoder
-        // and its "source of truth" behavior in relation to the relative encoder
-        // Probably doesn't *hurt* that it's here, but it may not be needed
+    public void setDesiredState(SwerveModuleVelocity desiredState) {
+        lastTargetState = new SwerveModuleVelocity(desiredState.velocity, desiredState.angle);
+        
         desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
 
         lastTargetStateOptimized = desiredState;
 
         drive.setControl(
             driveVelocityRequest.withVelocity(
-                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+                desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
             ).withFeedForward(
-                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+                desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
             )
         );
 
@@ -196,7 +206,7 @@ public class SwerveModule {
     }
 
     public double getTurningEncoderPosition() {
-        return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
+        return turningRelativeEncoder.getPosition().get() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
     }
 
     public void resetEncoders() {
@@ -213,3 +223,4 @@ public class SwerveModule {
         }
     }
 }
+

src/main/java/first/robot/utilities/Utilities.java

@@ -1,28 +1,77 @@
-package frc.robot.utilities;
+package first.robot.utilities;
 
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import java.util.Optional;
+
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.math.geometry.Pose2d;
+
+import first.robot.constants.CompetitionConstants;
 
 public class Utilities {
     public static final double kG = -9.81;
     
+    /**
+     * Returns the Alliance enumeration that indicates who will have the first
+     * shift. Returns null if the data is not available. 
+     * 
+     * @return The Alliance that will have the first shift, or null if game specific data
+     *  is not present
+     */
     public static Alliance whoHasFirstShift() {
-        String gameData = DriverStation.getGameSpecificMessage();
+        Optional<String> gameData = MatchState.getGameData();
 
-        if(gameData.length() > 0) {
-            switch (gameData.charAt(0)) {
-                case 'B':
-                    return Alliance.Red;
-                case 'R':
-                    return Alliance.Blue;
-                default:
-                    return null;
+        if(gameData.isPresent()) {
+            String gameDataString = gameData.get();
+
+            if(gameDataString.length() > 0) {
+                switch (gameDataString.charAt(0)) {
+                    case 'B':
+                        return Alliance.RED;
+                    case 'R':
+                        return Alliance.BLUE;
+                    default:
+                        return null;
+                }
             }
         }
-        
         return null;
     }
 
+    /**
+     * Returns the pose of the hub for the given alliance assigned to our robot.
+     * If no alliance is assigned (which is unlikely) this method returns
+     * the Blue hub pose, which is the closet to the field origin
+     * 
+     * @return The Pose2d object which represents the appropriate pose of the Hub
+     */
+    public static Pose2d getHubPose() {
+        Optional<Alliance> alliance = MatchState.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
+            return CompetitionConstants.kBlueHubLocation;
+        } else {
+            return CompetitionConstants.kRedHubLocation;
+        }
+    }
+
+    /**
+     * Returns the AprilTag ID of the tag that is in the center of the hub
+     * for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
+     * the Blue hub's center tag is returned. 
+     * 
+     * @return The tag ID that is in the center of the assigned alliance's hub
+     */
+    public static int getHubCenterAprilTagID() {
+        Optional<Alliance> alliance = MatchState.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
+            return 26;
+        } else {
+            return 10;
+        }
+    }
+
     /**
      * A ChatGPT possible hallucination related to calcuating whether a shot is possible
      * for a given speed and change in X and Y position
@@ -30,8 +79,6 @@ public class Utilities {
      * Note that X in this scenario is the physical distance from the shooter exit to 
      * target. Y is the change in height from the shooter exit to the target height
      * 
-     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
-     * 
      * @param targetVMPS The target velocity of the shooter in Meters per Second
      * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
      * @param deltaYM The height difference between the shooter exit and the target
@@ -52,8 +99,6 @@ public class Utilities {
      * Setting softerShot to true changes the angle of attack to a soft, long range shot. False
      * makes the shot more of a lob
      * 
-     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
-     * 
      * @param targetVMPS The target velocity of the shooter in Meters per Second
      * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
      * @param deltaYM The height difference between the shooter exit and the target

src/main/java/first/robot/utilities/VisualPose.java

@@ -0,0 +1,9 @@
+package first.robot.utilities;
+
+import org.wpilib.math.geometry.Pose2d;
+
+/**
+ * A record class which represents the source of a visual pose, the pose itself
+ * and the timestamp the pose was generated. 
+ */
+public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {}

src/main/java/frc/robot/Main.java

@@ -1,15 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-public final class Main {
-  private Main() {}
-
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}

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

@@ -1,101 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import org.littletonrobotics.junction.LogFileUtil;
-import org.littletonrobotics.junction.LoggedRobot;
-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;
-import frc.robot.constants.CompetitionConstants;
-
-public class Robot extends LoggedRobot {
-  private Command m_autonomousCommand;
-
-  private final RobotContainer m_robotContainer;
-
-  @SuppressWarnings("resource")
-  public Robot() {
-    Logger.recordMetadata("ProjectName", "2026_Robot_Code");
-
-    if(isReal()) {
-      if(CompetitionConstants.kLogToNetworkTables) {
-        Logger.addDataReceiver(new NT4Publisher());
-      }
-      
-      Logger.addDataReceiver(new WPILOGWriter());
-
-      new PowerDistribution(1, ModuleType.kRev);
-    } else {
-      setUseTiming(false);
-      String logPath = LogFileUtil.findReplayLog();
-      Logger.setReplaySource(new WPILOGReader(logPath));
-      Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
-    }
-
-    Logger.start();
-
-    m_robotContainer = new RobotContainer();
-  }
-
-  @Override
-  public void robotPeriodic() {
-    CommandScheduler.getInstance().run();
-  }
-
-  @Override
-  public void disabledInit() {}
-
-  @Override
-  public void disabledPeriodic() {}
-
-  @Override
-  public void disabledExit() {}
-
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    if (m_autonomousCommand != null) {
-      CommandScheduler.getInstance().schedule(m_autonomousCommand);
-    }
-  }
-
-  @Override
-  public void autonomousPeriodic() {}
-
-  @Override
-  public void autonomousExit() {}
-
-  @Override
-  public void teleopInit() {
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-  }
-
-  @Override
-  public void teleopPeriodic() {}
-
-  @Override
-  public void teleopExit() {}
-
-  @Override
-  public void testInit() {
-    CommandScheduler.getInstance().cancelAll();
-  }
-
-  @Override
-  public void testPeriodic() {}
-
-  @Override
-  public void testExit() {}
-}

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

@@ -1,126 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.constants.AutoConstants;
-import frc.robot.constants.OIConstants;
-import frc.robot.subsystems.Drivetrain;
-import frc.robot.utilities.Elastic;
-import frc.robot.utilities.Utilities;
-
-public class RobotContainer {
-  private Drivetrain drivetrain;
-
-  private CommandXboxController driver;
-
-  private SendableChooser<Command> autoChooser;
-
-  private Timer shiftTimer;
-
-  public RobotContainer() {
-    drivetrain = new Drivetrain();
-
-    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
-
-    shiftTimer = new Timer();
-    shiftTimer.reset();
-
-    configureBindings();
-  }
-  
-  private void configureBindings() {
-    drivetrain.setDefaultCommand(
-      drivetrain.drive(
-        driver::getLeftY,
-        driver::getLeftX, 
-        driver::getRightX, 
-        () -> false
-      )
-    );
-
-    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
-    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
-    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
-    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
-    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
-    driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
-    driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
-    driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
-    driver.rightBumper().whileTrue(drivetrain.setX());
-
-    //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
-
-    configureShiftDisplay();
-  }
-
-  public Command getAutonomousCommand() {
-    if(AutoConstants.kAutoConfigOk) {
-      return autoChooser.getSelected();
-    } else {
-      return new PrintCommand("Robot Config loading failed, autonomous disabled");
-    }
-  }
-
-  private void configureShiftDisplay() {
-    SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-    
-    RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
-      shiftTimer.stop();
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-    }));
-
-    RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
-      Elastic.selectTab(OIConstants.kTeleopTab);
-      shiftTimer.reset();
-      shiftTimer.start();
-    }));
-
-    new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-    }));
-
-    new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
-        OIConstants.kCurrentActiveHub, 
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
-      );
-    }));
-
-    new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
-        OIConstants.kCurrentActiveHub, 
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
-      );
-    }));
-
-    new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
-        OIConstants.kCurrentActiveHub, 
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
-      );
-    }));
-
-    new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
-        OIConstants.kCurrentActiveHub, 
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
-      );
-    }));
-
-    new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-    }));
-  }
-}

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

@@ -1,13 +0,0 @@
-package frc.robot.constants;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-
-public class CompetitionConstants {
-    // THIS SHOULD BE FALSE DURING COMPETITION PLAY
-    public static final boolean kLogToNetworkTables = true;
-
-    public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
-        AprilTagFields.kDefaultField
-    );
-}

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

@@ -1,43 +0,0 @@
-package frc.robot.constants;
-
-import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-public class HoodConstants {
-    // TODO Real Values
-    public static final int kMotorCANID = 0;
-
-    public static final double kP = 0;
-    public static final double kI = 0;
-    public static final double kD = 0;
-    public static final double kS = 0;
-    public static final double kV = 0;
-    public static final double kA = 0;
-    public static final double kStartupAngle = 0;
-
-    public static final int kCurrentLimit = 15;
-
-    public static final boolean kInverted = false;
-
-    public static final IdleMode kIdleMode = IdleMode.kBrake;
-
-    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
-
-    public static final SparkMaxConfig kConfig = new SparkMaxConfig();
-
-    static {
-        kConfig
-            .idleMode(kIdleMode)
-            .inverted(kInverted)
-            .smartCurrentLimit(kCurrentLimit);
-        kConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kP, kI, kD)
-            .outputRange(-1, 1)
-            .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, Math.PI * 2)
-            .feedForward
-                .sva(kS, kV, kA);
-    }
-}

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

@@ -1,69 +0,0 @@
-package frc.robot.constants;
-
-import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-public class IntakePivotConstants {
-    // TODO Real values
-    public enum IntakePivotPosition {
-        kUp(0),
-        kDown(0);
-        private double positionRadians;
-
-        private IntakePivotPosition(double positionRadians) {
-            this.positionRadians = positionRadians;
-        }
-
-        public double getPositionRadians() {
-            return positionRadians;
-        }
-    }
-
-    public static final int kLeftMotorCANID = 0;
-    public static final int kRightMotorCANID = 1;
-
-    public static final double kConversionFactor = 0;
-
-    public static final double kP = 0;
-    public static final double kI = 0;
-    public static final double kD = 0;
-    public static final double kS = 0;
-    public static final double kV = 0;
-    public static final double kA = 0;
-
-    public static final boolean kInvertMotors = false;
-
-    public static final int kCurrentLimit = 30;
-
-    public static final IdleMode kIdleMode = IdleMode.kBrake;
-
-    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
-    
-    public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
-
-    static {
-        KLeftMotorConfig
-            .idleMode(kIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
-        KLeftMotorConfig.absoluteEncoder
-            .positionConversionFactor(kConversionFactor)
-            .velocityConversionFactor(kConversionFactor / 60);
-        KLeftMotorConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kP, kI, kD)
-            .outputRange(-1, 1)
-            .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, 2 * Math.PI)
-            .feedForward
-                .sva(kS, kV, kA);
-
-        kRightMotorConfig
-            .idleMode(kIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
-            .follow(kLeftMotorCANID);
-    }
-}

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

@@ -1,17 +0,0 @@
-package frc.robot.constants;
-
-import edu.wpi.first.math.geometry.Transform3d;
-
-public class PhotonConstants {
-    public static final String kCamera1Name = "pv1";
-    public static final String kCamera2Name = "pv2";
-
-    // TODO Need actual values for all of this
-    public static final Transform3d kCamera1RobotToCam = new Transform3d();
-    public static final Transform3d kCamera2RobotToCam = new Transform3d();
-
-    public static final double kCamera1HeightMeters = 0;
-    public static final double kCamera1PitchRadians = 0;
-    public static final double kCamera2HeightMeters = 0;
-    public static final double kCamera2PitchRadians = 0;
-}

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

@@ -1,114 +0,0 @@
-package frc.robot.constants;
-
-import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-import edu.wpi.first.math.util.Units;
-
-public class ShooterConstants {
-    public enum ShooterSpeeds {
-        kHubSpeed(0, 0),
-        kFeedSpeed(0, 0);
-
-        private double frontRollerMPS;
-        private double rearRollerMPS;
-
-        private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
-            this.frontRollerMPS = frontRollerMPS;
-            this.rearRollerMPS = rearRollerMPS;
-        }
-
-        public double getFrontRollerMPS() {
-            return frontRollerMPS;
-        }
-
-        public double getRearRollerMPS() {
-            return rearRollerMPS;
-        }
-    }
-
-    // TODO Conversion factor?
-
-    public static final double kWheelDiameter = Units.inchesToMeters(6);
-
-    // TODO Real values
-    public static final int kFrontShooterMotor1CANID = 0;
-    public static final int kFrontShooterMotor2CANID = 0;
-    public static final int kRearShooterMotor1CANID = 0;
-    public static final int kRearShooterMotor2CANID = 0;
-
-    public static final boolean kFrontShooterMotor1Inverted = false;
-    public static final boolean kFrontShooterMotor2Inverted = false;
-    public static final boolean kRearShooterMotor1Inverted = false;
-    public static final boolean kRearShooterMotor2Inverted = false;
-
-    public static final double kFrontP = 0;
-    public static final double kFrontI = 0;
-    public static final double kFrontD = 0;
-    public static final double kFrontS = 0;
-    public static final double kFrontV = 0;
-    public static final double kFrontA = 0;
-
-    public static final double kRearP = 0;
-    public static final double kRearI = 0;
-    public static final double kRearD = 0;
-    public static final double kRearS = 0;
-    public static final double kRearV = 0;
-    public static final double kRearA = 0;
-
-    // TODO Is this value sane?
-    public static final int kCurrentLimit = 30;
-
-    public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
-
-    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
-
-    public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
-
-
-    static {
-        kFrontMotor1Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor1Inverted);
-        kFrontMotor1Config.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kFrontMotor1Config.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kFrontP, kFrontI, kFrontD)
-            .outputRange(-1, 1)
-            .feedForward
-                .sva(kFrontS, kFrontV, kFrontA);
-
-        kFrontMotor2Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor2Inverted)
-            .follow(kFrontShooterMotor1CANID);
-
-        kRearMotor1Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRearShooterMotor1Inverted);
-        kRearMotor1Config.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kRearMotor1Config.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kRearP, kRearI, kRearD)
-            .outputRange(-1, 1)
-            .feedForward
-                .sva(kRearS, kRearV, kRearA);
-
-        kRearMotor2Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRearShooterMotor2Inverted)
-            .follow(kRearShooterMotor1CANID);
-    }
-}

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -1,39 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.OptionalDouble;
-
-/**
- * An interface which ensures a class can provide common AprilTag oriented
- * information from various sources in a consistent way.
- */
-public interface IAprilTagProvider {
-    /**
-     * A method to get the tags currently in the camera's field of view
-     * @return
-     */
-    public int[] getVisibleTagIDs();
-
-    /**
-     * A method to get the distance from <i>the camera</i> to the AprilTag specified
-     * 
-     * @param id The ID of the AprilTag to give a distance to
-     * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagDistanceFromCameraByID(int id);
-
-    /**
-     * A method to get the pitch from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the pitch of
-     * @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagPitchByID(int id);
-
-    /**
-     * A method to get the yaw from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the yaw of
-     * @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagYawByID(int id);
-}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -1,27 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.Optional;
-
-import edu.wpi.first.math.geometry.Pose2d;
-
-/**
- * An interface which ensures a class' ability to provide visual pose information
- * in a consistent way
- */
-public interface IVisualPoseProvider {
-    /**
-     * A record that can contain the two elements necessary for a WPILIB
-     * pose estimator to use the information from a vision system as part of a full
-     * robot pose estimation
-     */
-    public record VisualPose(Pose2d visualPose, double timestamp) {}
-
-    /**
-     * Return a VisualPose or null if an empty Optional if none is available.
-     * Implementation should provide an empty response if it's unable to provide
-     * a reliable pose, or any pose at all.
-     * 
-     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
-     */
-    public Optional<VisualPose> getVisualPose();
-}

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

@@ -1,313 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.Optional;
-import java.util.OptionalDouble;
-import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathPlannerPath;
-import com.studica.frc.AHRS;
-import com.studica.frc.AHRS.NavXComType;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-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.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
-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.utilities.PhotonVision;
-import frc.robot.utilities.SwerveModule;
-
-public class Drivetrain extends SubsystemBase {
-    private SwerveModule frontLeft;
-    private SwerveModule frontRight;
-    private SwerveModule rearLeft;
-    private SwerveModule rearRight;
-
-    private AHRS gyro;
-
-    private SwerveDrivePoseEstimator estimator;
-
-    private PhotonVision camera1;
-    private PhotonVision camera2;
-
-    public Drivetrain() {
-        frontLeft = new SwerveModule(
-            "FrontLeft", 
-            DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID, 
-            DrivetrainConstants.kFrontLeftAnalogInPort, 
-            DrivetrainConstants.kFrontLeftMagEncoderOffset
-        );
-
-        frontRight = new SwerveModule(
-            "FrontRight", 
-            DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID, 
-            DrivetrainConstants.kFrontRightAnalogInPort, 
-            DrivetrainConstants.kFrontRightMagEncoderOffset
-        );
-
-        rearLeft = new SwerveModule(
-            "RearLeft", 
-            DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID, 
-            DrivetrainConstants.kRearLeftAnalogInPort, 
-            DrivetrainConstants.kRearLeftMagEncoderOffset
-        );
-
-        rearRight = new SwerveModule(
-            "RearRight", 
-            DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID, 
-            DrivetrainConstants.kRearRightAnalogInPort, 
-            DrivetrainConstants.kRearRightMagEncoderOffset
-        );
-
-        gyro = new AHRS(NavXComType.kMXP_SPI);
-
-        // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
-        estimator = new SwerveDrivePoseEstimator(
-            DrivetrainConstants.kDriveKinematics, 
-            Rotation2d.fromDegrees(getGyroValue()), 
-            new SwerveModulePosition[] {
-                frontLeft.getPosition(),
-                frontRight.getPosition(),
-                rearLeft.getPosition(),
-                rearRight.getPosition()
-            },
-            new Pose2d()
-        );
-
-        if(AutoConstants.kAutoConfigOk) {
-            AutoBuilder.configure(
-                this::getPose, 
-                this::resetOdometry, 
-                this::getCurrentChassisSpeeds, 
-                (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), 
-                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() {
-        estimator.update(
-            Rotation2d.fromDegrees(getGyroValue()),
-            new SwerveModulePosition[] {
-                frontLeft.getPosition(),
-                frontRight.getPosition(),
-                rearLeft.getPosition(),
-                rearRight.getPosition()
-            }
-        );
-
-        frontLeft.periodic();
-        frontRight.periodic();
-        rearLeft.periodic();
-        rearRight.periodic();
-
-        Logger.recordOutput("Drivetrain/Pose", getPose());
-        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
-        Logger.recordOutput("Drivetrain/Heading", getHeading());
-    }
-
-    public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            frontLeft.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-    public Command runFrontRight(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            frontRight.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-    public Command runRearLeft(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            rearLeft.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-    public Command runRearRight(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            rearRight.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-
-    public Command disableOutputs() {
-        return run(() -> {
-            frontLeft.disableOutput();
-            frontRight.disableOutput();
-            rearLeft.disableOutput();
-            rearRight.disableOutput();
-        });  
-    }
-
-    // TODO check both cameras
-    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
-        if (camera1 == null) {
-            return new PrintCommand("Camera 1 not available");
-        }
-
-        // TODO The process variable is different here than what these constants are used for, may need to use something different
-        PIDController controller = new PIDController(
-            AutoConstants.kPThetaController, 
-            0, 
-            0
-        );
-
-        return runOnce(controller::reset).andThen(
-            drive(
-                xSpeed, 
-                ySpeed, 
-                () -> {
-                    OptionalDouble tagYaw = camera1.getTagYawByID(tagID);
-
-                    return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0));
-                },
-                () -> false
-           )
-        );
-    }
-
-    public Command drivePathPlannerPath(PathPlannerPath path) {
-        if(AutoConstants.kAutoConfigOk) {
-            return AutoBuilder.followPath(path);
-        } else {
-            return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled");
-        }
-        
-    }
-
-    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
-        // TODO Specific Alliance code?
-        return run(() -> {
-            drive(
-                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
-                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
-                fieldRelative.getAsBoolean()
-            );
-        });
-    }
-
-    public Command setX() {
-        return run(() -> {
-            frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-            frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-            rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-            rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-        });
-    }
-
-    public void resetEncoders() {
-        frontLeft.resetEncoders();
-        frontRight.resetEncoders();
-        rearLeft.resetEncoders();
-        rearRight.resetEncoders();
-    }
-
-    public void resetOdometry(Pose2d pose) {
-        estimator.resetPosition(
-            Rotation2d.fromDegrees(getGyroValue()),
-            new SwerveModulePosition[] {
-                frontLeft.getPosition(),
-                frontRight.getPosition(),
-                rearLeft.getPosition(),
-                rearRight.getPosition()
-            },
-            pose
-        );
-    }
-
-    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
-        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
-        double xSpeedDelivered = 0;
-        double ySpeedDelivered = 0;
-
-        if(p != 0){
-            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
-            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
-        }else{
-            xSpeedDelivered = 0;
-            ySpeedDelivered = 0;
-        }
-
-        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
-
-        SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
-            fieldRelative ?
-                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    estimator.getEstimatedPosition().getRotation()) :
-                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
-        );
-
-        setModuleStates(swerveModuleStates);
-    }
-
-    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
-        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
-
-        setModuleStates(newStates);
-    }
-
-    public void setModuleStates(SwerveModuleState[] desiredStates) {
-        SwerveDriveKinematics.desaturateWheelSpeeds(
-            desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
-        frontLeft.setDesiredState(desiredStates[0]);
-        frontRight.setDesiredState(desiredStates[1]);
-        rearLeft.setDesiredState(desiredStates[2]);
-        rearRight.setDesiredState(desiredStates[3]);
-    }
-
-    public ChassisSpeeds getCurrentChassisSpeeds() {
-        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
-            frontLeft.getState(),
-            frontRight.getState(),
-            rearLeft.getState(),
-            rearRight.getState()
-        );
-    }
-
-    public Pose2d getPose() {
-        return estimator.getEstimatedPosition();
-    }
-
-    public double getGyroValue() {
-        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
-    }
-
-    public double getHeading() {
-        return estimator.getEstimatedPosition().getRotation().getDegrees();
-    }
-}

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

@@ -1,60 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.HoodConstants;
-
-public class Hood extends SubsystemBase {
-    private SparkMax motor;
-
-    private AbsoluteEncoder encoder;
-
-    private SparkClosedLoopController controller;
-
-    private double currentTargetRadians;
-
-    public Hood() {
-        motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
-
-        encoder = motor.getAbsoluteEncoder();
-
-        controller = motor.getClosedLoopController();
-
-        currentTargetRadians = HoodConstants.kStartupAngle;
-    }
-
-    @Override
-    public void periodic() {
-        Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
-        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
-        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
-    }
-
-    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
-        return run(() -> {
-            currentTargetRadians = radianAngleSupplier.getAsDouble();
-
-            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
-        });
-    }
-
-    public Command stop() {
-        return run(() -> {
-            motor.disable();
-        });
-    }
-
-    public double getTargetRadians() {
-        return currentTargetRadians;
-    }
-}

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

@@ -1,79 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.Optional;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IntakePivotConstants;
-import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
-
-public class IntakePivot extends SubsystemBase {
-    private SparkMax leftMotor;
-    private SparkMax rightMotor;
-
-    private AbsoluteEncoder encoder;
-
-    private SparkClosedLoopController controller;
-
-    private IntakePivotPosition currentTargetPosition;
-
-    public IntakePivot() {
-        leftMotor = new SparkMax(IntakePivotConstants.kLeftMotorCANID, MotorType.kBrushless);
-        rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless);
-
-        leftMotor.configure(
-            IntakePivotConstants.KLeftMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        rightMotor.configure(
-            IntakePivotConstants.kRightMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        controller = leftMotor.getClosedLoopController();
-
-        encoder = leftMotor.getAbsoluteEncoder();
-    }
-
-    @Override
-    public void periodic() {
-        Logger.recordOutput(
-            "IntakePivot/TargetPosition", 
-            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
-        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
-        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
-    }
-
-    public Command maintainPosition(IntakePivotPosition position) {
-        currentTargetPosition = position;
-
-        return run(() -> {
-            if(currentTargetPosition == null) {
-                leftMotor.disable();
-            } else {
-                controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
-            }
-        });
-    }
-
-    public Command stop() {
-        return maintainPosition(null);
-    }
-
-    public Optional<IntakePivotPosition> getCurrentTargetPosition() {
-        return Optional.ofNullable(currentTargetPosition);
-    }
-}

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

@@ -1,51 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IntakeRollerConstants;
-
-public class IntakeRoller extends SubsystemBase {
-    private SparkMax leftMotor;
-    private SparkMax rightMotor;
-
-    public IntakeRoller() {
-        leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
-        rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
-
-        leftMotor.configure(
-            IntakeRollerConstants.leftMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        rightMotor.configure(
-            IntakeRollerConstants.rightMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-    }
-
-    public Command runIn() {
-        return run(() -> {
-            leftMotor.set(1);
-        });
-    }
-
-    public Command runOut() {
-        return run(() -> {
-            leftMotor.set(-1);
-        });
-    }
-
-    public Command stop() {
-        return run(() -> {
-            leftMotor.set(0);
-        });
-    }
-    
-}

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

@@ -1,122 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.Optional;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ShooterConstants;
-import frc.robot.constants.ShooterConstants.ShooterSpeeds;
-
-public class Shooter extends SubsystemBase {
-    private SparkMax frontMotor1;
-    private SparkMax frontMotor2;
-    private SparkMax rearMotor1;
-    private SparkMax rearMotor2;
-
-    private AbsoluteEncoder frontEncoder;
-    private AbsoluteEncoder rearEncoder;
-
-    private SparkClosedLoopController frontClosedLoopController;
-    private SparkClosedLoopController rearClosedLoopController;
-
-    private ShooterSpeeds targetSpeeds;
-
-    public Shooter() {
-        frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
-        frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
-        rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
-        rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
-
-        frontMotor1.configure(
-            ShooterConstants.kFrontMotor1Config, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        rearMotor1.configure(
-            ShooterConstants.kRearMotor1Config, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        frontMotor2.configure(
-            ShooterConstants.kFrontMotor2Config, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        rearMotor2.configure(
-            ShooterConstants.kRearMotor2Config, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        frontEncoder = frontMotor1.getAbsoluteEncoder();
-        rearEncoder = rearMotor1.getAbsoluteEncoder();
-
-        frontClosedLoopController = frontMotor1.getClosedLoopController();
-        rearClosedLoopController = rearMotor1.getClosedLoopController();
-
-        // TODO Set this to the initial startup speed
-        targetSpeeds = null;
-    }
-
-    @Override
-    public void periodic() {
-        Logger.recordOutput(
-            "Shooter/FrontRollers/TargetMPS", 
-            targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
-        );
-
-        Logger.recordOutput(
-            "Shooter/RearRollers/TargetMPS", 
-            targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
-        );
-
-        Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
-        Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
-
-        // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
-        Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
-        Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
-    }
-
-    public Command maintainSpeed(ShooterSpeeds speeds) {
-        targetSpeeds = speeds;
-
-        return run(() -> {
-            if(targetSpeeds == null) {
-                frontMotor1.disable();
-                rearMotor1.disable();
-            } else {
-                frontClosedLoopController.setSetpoint(
-                    targetSpeeds.getFrontRollerMPS(), 
-                    ControlType.kVelocity
-                );
-
-                rearClosedLoopController.setSetpoint(
-                    targetSpeeds.getRearRollerMPS(), 
-                    ControlType.kVelocity
-                );
-            }
-        });
-    }
-
-    public Command stop() {
-        return maintainSpeed(null);
-    }
-
-    public Optional<ShooterSpeeds> getTargetSpeeds() {
-        return Optional.ofNullable(targetSpeeds);
-    }
-}

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

@@ -1,59 +0,0 @@
-package frc.robot.subsystems;
-
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.SpindexerConstants;
-
-public class Spindexer extends SubsystemBase {
-    private TalonFX spindexerMotor;
-
-    private SparkMax feederMotor;
-
-    private DutyCycleOut spindexerMotorOutput;
-
-    public Spindexer() {
-        spindexerMotor = new TalonFX(SpindexerConstants.kSpindexerMotorCANID);
-
-        feederMotor = new SparkMax(SpindexerConstants.kFeederMotorCANID, MotorType.kBrushless);
-
-        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerCurrentLimitConfig);
-        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerMotorConfig);
-
-        feederMotor.configure(
-            SpindexerConstants.kFeederConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        spindexerMotorOutput = new DutyCycleOut(0);
-    }
-
-    public Command spinToShooter() {
-        return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(1));
-            feederMotor.set(1);
-        });
-    }
-
-    public Command spinToIntake() {
-        return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1));
-            feederMotor.set(-1);
-        });
-    }
-
-    public Command stop() {
-        return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
-            feederMotor.set(0);
-        });
-    }
-    
-}

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -1,165 +0,0 @@
-package frc.robot.utilities;
-
-import java.io.IOException;
-import java.util.List;
-import java.util.Optional;
-import java.util.OptionalDouble;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.PhotonUtils;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-import frc.robot.constants.CompetitionConstants;
-import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-
-public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
-
-    private final PhotonCamera camera;
-
-    private final PhotonPoseEstimator photonPoseEstimator;
-
-    private final double cameraHeightMeters;
-    private final double cameraPitchRadians;
-
-    private PhotonPipelineResult latestResult;
-
-    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
-        camera = new PhotonCamera(cameraName);
-
-        photonPoseEstimator = new PhotonPoseEstimator(
-            CompetitionConstants.kTagLayout, 
-            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
-            robotToCam
-        );
-
-        this.cameraHeightMeters = cameraHeightMeters;
-        this.cameraPitchRadians = cameraPitchRadians;
-
-        this.latestResult = null;
-    }
-
-    // TODO This periodic method has to be called from somewhere, even though the cameras
-    // could be used in multiple other subsystems
-    public void periodic() {
-        // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong
-        List<PhotonPipelineResult> results = camera.getAllUnreadResults();
-
-        if(!results.isEmpty()) {
-            latestResult = results.get(results.size() - 1);
-        }
-    }
-
-    @Override
-    public Optional<VisualPose> getVisualPose() {
-        if(latestResult == null) {
-            return Optional.empty();
-        }
-
-        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update(latestResult);
-
-        if (pose.isEmpty()) {
-            return Optional.empty();
-        }
-
-        return Optional.of(
-            new VisualPose(
-                pose.get().estimatedPose.toPose2d(), 
-                pose.get().timestampSeconds
-            )
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
-        if (latestResult == null) {
-            return OptionalDouble.empty();
-        }
-
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getYaw()
-        );
-    }
-
-    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
-        for (PhotonTrackedTarget target : targets) {
-            if (target.getFiducialId() == id) {
-                return Optional.of(target);
-            }
-        }
-
-        return Optional.empty();
-    }
-
-    @Override
-    public int[] getVisibleTagIDs() {
-        if(latestResult == null) {
-            return new int[] {};
-        }
-
-        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
-    }
-    
-}

src/main/java/frc/robot/utilities/SparkMAXTester.java

@@ -1,23 +0,0 @@
-package frc.robot.utilities;
-
-import java.util.function.DoubleSupplier;
-
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class SparkMAXTester extends SubsystemBase {
-    private SparkMax spark;
-
-    public SparkMAXTester(int deviceID) {
-        spark = new SparkMax(deviceID, MotorType.kBrushless);
-    }
-
-    public Command setSpeed(DoubleSupplier speed) {
-        return run(() -> {
-            spark.set(speed.getAsDouble());
-        });
-    }
-}

vendordeps/AdvantageKit.json

@@ -1,35 +0,0 @@
-{
-    "fileName": "AdvantageKit.json",
-    "name": "AdvantageKit",
-    "version": "26.0.0",
-    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
-    "frcYear": "2026",
-    "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": "26.0.0"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "org.littletonrobotics.akit",
-            "artifactId": "akit-wpilibio",
-            "version": "26.0.0",
-            "skipInvalidPlatforms": false,
-            "isJar": false,
-            "validPlatforms": [
-                "linuxathena",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal",
-                "windowsx86-64"
-            ]
-        }
-    ],
-    "cppDependencies": []
-}

vendordeps/CommandsV2.json

@@ -1,36 +1,41 @@
 {
-  "fileName": "WPILibNewCommands.json",
-  "name": "WPILib-New-Commands",
+  "fileName": "CommandsV2.json",
+  "name": "Commands V2",
   "version": "1.0.0",
   "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
-  "frcYear": "2026",
+  "wpilibYear": "2027_alpha7",
   "mavenUrls": [],
   "jsonUrl": "",
+  "conflictsWith": [
+    {
+      "uuid": "4decdc05-a056-46cf-9561-39449bbb01306",
+      "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.",
+      "offlineFileName": "CommandsV3.json"
+    }
+  ],
   "javaDependencies": [
     {
-      "groupId": "edu.wpi.first.wpilibNewCommands",
-      "artifactId": "wpilibNewCommands-java",
+      "groupId": "org.wpilib.commandsv2",
+      "artifactId": "commandsv2-java",
       "version": "wpilib"
     }
   ],
   "jniDependencies": [],
   "cppDependencies": [
     {
-      "groupId": "edu.wpi.first.wpilibNewCommands",
-      "artifactId": "wpilibNewCommands-cpp",
+      "groupId": "org.wpilib.commandsv2",
+      "artifactId": "commandsv2-cpp",
       "version": "wpilib",
-      "libName": "wpilibNewCommands",
+      "libName": "commandsv2",
       "headerClassifier": "headers",
       "sourcesClassifier": "sources",
       "sharedLibrary": true,
       "skipInvalidPlatforms": true,
       "binaryPlatforms": [
         "linuxsystemcore",
-        "linuxathena",
-        "linuxarm32",
         "linuxarm64",
         "windowsx86-64",
-        "windowsx86",
+        "windowsarm64",
         "linuxx86-64",
         "osxuniversal"
       ]

vendordeps/PathplannerLibSystemCoreAlpha.json

@@ -1,18 +1,18 @@
 {
-    "fileName": "PathplannerLib-2026.1.2.json",
+    "fileName": "PathplannerLibSystemCoreAlpha.json",
     "name": "PathplannerLib",
-    "version": "2026.1.2",
+    "version": "2027.0.0-alpha-4.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
-    "frcYear": "2026",
+    "wpilibYear": "2027_alpha7",
     "mavenUrls": [
         "https://3015rangerrobotics.github.io/pathplannerlib/repo"
     ],
-    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLibSystemCoreAlpha.json",
     "javaDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2026.1.2"
+            "version": "2027.0.0-alpha-4.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2026.1.2",
+            "version": "2027.0.0-alpha-4.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -29,8 +29,7 @@
                 "windowsx86-64",
                 "linuxx86-64",
                 "osxuniversal",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "linuxarm64"
             ]
         }

vendordeps/Phoenix5-5.36.0.json

@@ -1,171 +0,0 @@
-{
-    "fileName": "Phoenix5-5.36.0.json",
-    "name": "CTRE-Phoenix (v5)",
-    "version": "5.36.0",
-    "frcYear": "2026",
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-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-frc2026-latest.json",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-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-frc2026-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-frc2026-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-java",
-            "version": "5.36.0"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-java",
-            "version": "5.36.0"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.36.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.36.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-cpp",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixCCI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_Phoenix_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixSim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.36.0",
-            "libName": "CTRE_PhoenixCCISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}

vendordeps/Phoenix6-26.70.0-alpha-2.json

@@ -1,60 +1,60 @@
 {
-    "fileName": "Phoenix6-26.1.0.json",
+    "fileName": "Phoenix6-26.70.0-alpha-2.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "26.1.0",
-    "frcYear": "2026",
+    "version": "26.70.0-alpha-2",
+    "wpilibYear": "2027_alpha7",
     "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-frc2026-latest.json",
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2027-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-frc2026-latest.json"
+            "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+            "offlineFileName": "Phoenix6-replay-frc2027-latest.json"
         }
     ],
     "javaDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "26.1.0"
+            "version": "26.70.0-alpha-2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
                 "windowsx86-64",
                 "linuxx86-64",
                 "linuxarm64",
-                "linuxathena"
+                "linuxsystemcore"
             ],
             "simMode": "hwsim"
         },
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
                 "windowsx86-64",
                 "linuxx86-64",
                 "linuxarm64",
-                "linuxathena"
+                "linuxsystemcore"
             ],
             "simMode": "hwsim"
         },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -208,7 +208,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdle",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -224,7 +224,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -233,14 +233,14 @@
                 "windowsx86-64",
                 "linuxx86-64",
                 "linuxarm64",
-                "linuxathena"
+                "linuxsystemcore"
             ],
             "simMode": "hwsim"
         },
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -249,14 +249,14 @@
                 "windowsx86-64",
                 "linuxx86-64",
                 "linuxarm64",
-                "linuxathena"
+                "linuxsystemcore"
             ],
             "simMode": "hwsim"
         },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -272,7 +272,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -288,7 +288,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -304,7 +304,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -320,7 +320,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -336,7 +336,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -352,7 +352,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -368,7 +368,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,7 +384,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -400,7 +400,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -416,7 +416,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProCANdi",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -432,7 +432,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdle",
-            "version": "26.1.0",
+            "version": "26.70.0-alpha-2",
             "libName": "CTRE_SimProCANdle",
             "headerClassifier": "headers",
             "sharedLibrary": true,

vendordeps/REVLib.json

@@ -1,63 +1,60 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2026.0.0",
-    "frcYear": "2026",
+    "version": "2027.0.0-alpha-8",
+    "wpilibYear": "2027_alpha7",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
         "https://maven.revrobotics.com/"
     ],
-    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json",
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2027.json",
     "javaDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2026.0.0"
+            "version": "2027.0.0-alpha-8"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         }
@@ -66,7 +63,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -75,15 +72,14 @@
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -92,15 +88,14 @@
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "libName": "BackendDriver",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -108,15 +103,14 @@
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.0",
+            "version": "2027.0.0-alpha-8",
             "libName": "REVLibWpi",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -124,8 +118,7 @@
                 "windowsx86-64",
                 "linuxarm64",
                 "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
+                "linuxsystemcore",
                 "osxuniversal"
             ]
         }

vendordeps/Studica.json

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

vendordeps/photonlib-v2027.0.0-alpha-2.json

@@ -0,0 +1,84 @@
+{
+  "fileName": "photonlib.json",
+  "name": "photonlib",
+  "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
+  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+  "wpilibYear": "2027_alpha7",
+  "mavenUrls": [
+    "https://maven.photonvision.org/repository/internal",
+    "https://maven.photonvision.org/repository/snapshots"
+  ],
+  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+  "jniDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxsystemcore",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.wpilib.fields",
+      "artifactId": "fields-cpp",
+      "version": "wpilib",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxsystemcore",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "cppDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-cpp",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
+      "libName": "photonlib",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxsystemcore",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870",
+      "libName": "photontargeting",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxsystemcore",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-java",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870"
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-java",
+      "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870"
+    }
+  ]
+}

vendordeps/photonlib.json

@@ -1,71 +0,0 @@
-{
-    "fileName": "photonlib.json",
-    "name": "photonlib",
-    "version": "v2026.0.1-beta",
-    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
-    "frcYear": "2026",
-    "mavenUrls": [
-        "https://maven.photonvision.org/repository/internal",
-        "https://maven.photonvision.org/repository/snapshots"
-    ],
-    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
-    "jniDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "photontargeting-cpp",
-            "version": "v2026.0.1-beta",
-            "skipInvalidPlatforms": true,
-            "isJar": false,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxathena",
-                "linuxx86-64",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "photonlib-cpp",
-            "version": "v2026.0.1-beta",
-            "libName": "photonlib",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxathena",
-                "linuxx86-64",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "photontargeting-cpp",
-            "version": "v2026.0.1-beta",
-            "libName": "photontargeting",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxathena",
-                "linuxx86-64",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "photonlib-java",
-            "version": "v2026.0.1-beta"
-        },
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "photontargeting-java",
-            "version": "v2026.0.1-beta"
-        }
-    ]
-}